Skip to content
Snippets Groups Projects
Commit 9534a7cd authored by bpwilcox's avatar bpwilcox
Browse files

fix rebase/merge conflicts 

deleting robot package
parent 6d4d92b4
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 47 additions and 746 deletions
nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp
+ 1
− 1
View file @ 9534a7cd
......@@ -86,7 +86,7 @@ public:
protected:
void cleanup()
{
robot_.reset();
robot_state_.reset();
}
private:
......
nav2_dwb_controller/dwb_controller/include/dwb_controller/dwb_controller.hpp
+ 0
− 3
View file @ 9534a7cd
......@@ -23,13 +23,10 @@
#include "dwb_core/dwb_local_planner.hpp"
#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
#include "nav_2d_utils/odom_subscriber.hpp"
<<<<<<< HEAD
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_msgs/action/follow_path.hpp"
#include "nav2_util/simple_action_server.hpp"
=======
#include "nav2_util/robot_utils.hpp"
>>>>>>> changing the controller to use the nav2_utils velocity publisher wrapper as well
namespace dwb_controller
{
......
nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp
+ 2
− 4
View file @ 9534a7cd
......@@ -74,7 +74,7 @@ DwbController::on_configure(const rclcpp_lifecycle::State & state)
planner_->on_configure(state);
odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(*this);
vel_publisher_ = std::make_shared<nav2_util::VelocityPublisher>(temp_node);
vel_publisher_ = std::make_shared<nav2_util::VelocityPublisher>(rclcpp_node_);
// Create the action server that we implement with our followPath method
action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "FollowPath",
......@@ -90,7 +90,6 @@ DwbController::on_activate(const rclcpp_lifecycle::State & state)
planner_->on_activate(state);
costmap_ros_->on_activate(state);
vel_pub_->on_activate();
return nav2_util::CallbackReturn::SUCCESS;
}
......@@ -102,7 +101,6 @@ DwbController::on_deactivate(const rclcpp_lifecycle::State & state)
planner_->on_deactivate(state);
costmap_ros_->on_deactivate(state);
vel_pub_->on_deactivate();
return nav2_util::CallbackReturn::SUCCESS;
}
......@@ -119,7 +117,7 @@ DwbController::on_cleanup(const rclcpp_lifecycle::State & state)
// Release any allocated resources
planner_.reset();
odom_sub_.reset();
vel_pub_.reset();
vel_publisher_.reset();
action_server_.reset();
return nav2_util::CallbackReturn::SUCCESS;
......
nav2_motion_primitives/include/nav2_motion_primitives/motion_primitive.hpp
+ 2
− 1
View file @ 9534a7cd
......@@ -94,6 +94,7 @@ protected:
{
robot_state_.reset();
action_server_.reset();
vel_publisher_.reset();
}
void execute()
......@@ -156,7 +157,7 @@ protected:
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
vel_publisher_->sendVelocity(cmd_vel);
vel_publisher_->publishCommand(cmd_vel);
}
};
......
nav2_navfn_planner/src/navfn_planner.cpp
+ 0
− 4
View file @ 9534a7cd
......@@ -84,11 +84,7 @@ NavfnPlanner::on_configure(const rclcpp_lifecycle::State & /*state*/)
plan_marker_publisher_ = create_publisher<visualization_msgs::msg::Marker>(
"endpoints", 1);
<<<<<<< HEAD
auto node = shared_from_this();
=======
robot_state_ = std::make_unique<nav2_util::RobotStateHelper>(temp_node);
>>>>>>> changing all users of robot_ to their appropriate eq.
// Create the action server that we implement with our navigateToPose method
action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "ComputePathToPose",
......
nav2_robot/CMakeLists.txt deleted 100644 → 0
+ 0
− 55
View file @ 6d4d92b4
cmake_minimum_required(VERSION 3.5)
project(nav2_robot)
find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(urdf REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(nav2_util REQUIRED)
nav2_package()
include_directories(
include
)
add_library(nav2_robot SHARED
src/robot.cpp
)
ament_target_dependencies(nav2_robot
rclcpp
rclcpp_lifecycle
urdf
geometry_msgs
nav_msgs
nav2_util
)
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/
DESTINATION include/
)
if(BUILD_TESTING)
find_package(ament_cmake_pytest REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
add_subdirectory(test)
endif()
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(urdf)
ament_export_dependencies(nav_msgs)
ament_package()
nav2_robot/README.md deleted 100644 → 0
+ 0
− 14
View file @ 6d4d92b4
# Nav2 Robot
`nav2_robot` is a package containing an implementation class `Robot` containing information about robot specific parameters, controls, and information.
Today `Robot` contains basics for interacting with the robot hardware abstraction. It is the entry point for getting information from the odometry measurements and publishing velocity commands.
## ROS1 Comparison
This package does not have a direct counter-part in Navigation. Navigation contains a set of parameter files and reads information from each as needed. This package's intention is to act as a consolidator of robot-specific needs and information for each package across the meta-package to use. An example of another addition to meet that is to have an abstract implementation of a motion model for use in AMCL rather than hard-coding motion models options in the package itself.
## Future
* This package may soon be deprecated in favor of some robot-specific utilities
* Adding motion model, footprint, base parameters, etc
nav2_robot/include/nav2_robot/robot.hpp deleted 100644 → 0
+ 0
− 91
View file @ 6d4d92b4
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_ROBOT__ROBOT_HPP_
#define NAV2_ROBOT__ROBOT_HPP_
#include <string>
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include "urdf/model.h"
namespace nav2_robot
{
class Robot : public nav2_util::LifecycleHelperInterface
{
public:
explicit Robot(nav2_util::LifecycleNode::SharedPtr node);
explicit Robot(
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
bool auto_start = false);
Robot() = delete;
~Robot();
bool getGlobalLocalizerPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
bool getCurrentPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
bool getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom);
std::string getName();
void sendVelocity(geometry_msgs::msg::Twist twist);
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
protected:
// Interfaces used for logging and creating publishers and subscribers
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
// Publishers and subscribers
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
// Subscription callbacks
void onPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
void onOdomReceived(const nav_msgs::msg::Odometry::SharedPtr msg);
// The current pose as received from the Pose subscription
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose_;
// The odometry as received from the Odometry subscription
nav_msgs::msg::Odometry::SharedPtr current_odom_;
// Whether the subscriptions have been received
bool initial_pose_received_{false};
bool initial_odom_received_{false};
// Information about the robot is contained in the URDF file
std::string urdf_file_;
urdf::Model model_;
// Auto-start feature for non-lifecycle nodes
bool auto_start_;
void configure();
void cleanup();
};
} // namespace nav2_robot
#endif // NAV2_ROBOT__ROBOT_HPP_
nav2_robot/package.xml deleted 100644 → 0
+ 0
− 39
View file @ 6d4d92b4
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_robot</name>
<version>0.1.5</version>
<description>TODO</description>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
<maintainer email="mohammad.haghighipanah@intel.com">Mohammad Haghighipanah</maintainer>
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rclcpp</build_depend>
<build_depend>rclcpp_lifecycle</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>urdf</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>nav2_util</build_depend>
<build_depend>nav2_common</build_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>rclcpp_lifecycle</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>nav2_util</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>launch</test_depend>
<test_depend>launch_testing</test_depend>
<test_depend>nav2_util</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
nav2_robot/src/robot.cpp deleted 100644 → 0
+ 0
− 193
View file @ 6d4d92b4
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_robot/robot.hpp"
#include <string>
#include <exception>
#include "rclcpp/create_subscription.hpp"
#include "rclcpp/create_publisher.hpp"
#include "rclcpp/qos.hpp"
#include "urdf/model.h"
namespace nav2_robot
{
Robot::Robot(nav2_util::LifecycleNode::SharedPtr node)
: Robot(node->get_node_base_interface(),
node->get_node_topics_interface(),
node->get_node_logging_interface(),
false)
{
}
Robot::Robot(
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
bool auto_start)
: node_base_(node_base),
node_topics_(node_topics),
node_logging_(node_logging),
auto_start_(auto_start)
{
RCLCPP_INFO(node_logging_->get_logger(), "Robot: Creating");
if (auto_start_) {
configure();
}
}
Robot::~Robot()
{
RCLCPP_INFO(node_logging_->get_logger(), "Robot: Destroying");
if (auto_start_) {
cleanup();
}
}
nav2_util::CallbackReturn
Robot::on_configure(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(node_logging_->get_logger(), "Robot: Configuring");
configure();
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
Robot::on_activate(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(node_logging_->get_logger(), "Robot: Activating");
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
Robot::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(node_logging_->get_logger(), "Robot: Deactivating");
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
Robot::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(node_logging_->get_logger(), "Robot: Cleaning up");
cleanup();
return nav2_util::CallbackReturn::SUCCESS;
}
void
Robot::configure()
{
// This class may be used from a module that comes up after AMCL has output
// its initial pose, so that pose message uses durability TRANSIENT_LOCAL
pose_sub_ = rclcpp::create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
node_topics_, "amcl_pose",
rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
std::bind(&Robot::onPoseReceived, this, std::placeholders::_1));
odom_sub_ = rclcpp::create_subscription<nav_msgs::msg::Odometry>(
node_topics_, "odom", rclcpp::SensorDataQoS(),
std::bind(&Robot::onOdomReceived, this, std::placeholders::_1));
vel_pub_ = rclcpp::create_publisher<geometry_msgs::msg::Twist>(
node_topics_, "/cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)));
}
void
Robot::cleanup()
{
pose_sub_.reset();
odom_sub_.reset();
vel_pub_.reset();
}
void
Robot::onPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
// TODO(mjeronimo): serialize access
current_pose_ = msg;
if (!initial_pose_received_) {
initial_pose_received_ = true;
}
}
void
Robot::onOdomReceived(const nav_msgs::msg::Odometry::SharedPtr msg)
{
current_odom_ = msg;
if (!initial_odom_received_) {
initial_odom_received_ = true;
}
}
bool
Robot::getGlobalLocalizerPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose)
{
if (!initial_pose_received_) {
RCLCPP_DEBUG(node_logging_->get_logger(),
"Robot: Can't return current pose: Initial pose not yet received.");
return false;
}
robot_pose = current_pose_;
return true;
}
// TODO(mhpanah): We should get current pose from transforms.
bool
Robot::getCurrentPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose)
{
return getGlobalLocalizerPose(robot_pose);
}
bool
Robot::getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom)
{
if (!initial_odom_received_) {
RCLCPP_DEBUG(node_logging_->get_logger(),
"Robot: Can't return current velocity: Initial odometry not yet received.");
return false;
}
robot_odom = current_odom_;
return true;
}
// TODO(mhpanah): modify this method name and implementation to include robot types and Serial #(ID)
std::string
Robot::getName()
{
// Temporarily just returning a string until we enable parsing URDF file.
return "turtlebot";
}
void
Robot::sendVelocity(geometry_msgs::msg::Twist twist)
{
vel_pub_->publish(twist);
}
} // namespace nav2_robot
nav2_robot/test/CMakeLists.txt deleted 100644 → 0
+ 0
− 12
View file @ 6d4d92b4
include_directories( ${PROJECT_SOURCE_DIR}/src )
find_package(nav2_util REQUIRED)
ament_add_gtest(test_robot_class
test_robot_class.cpp
)
ament_target_dependencies(test_robot_class
${dependencies} nav2_util
)
target_link_libraries(test_robot_class nav2_robot)
nav2_robot/test/test_robot_class.cpp deleted 100644 → 0
+ 0
− 267
View file @ 6d4d92b4
// Copyright (c) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <string>
#include <memory>
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "nav2_robot/robot.hpp"
#include "nav2_util/lifecycle_service_client.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp/rclcpp.hpp"
using std::placeholders::_1;
using lifecycle_msgs::msg::Transition;
class RclCppFixture
{
public:
RclCppFixture() {rclcpp::init(0, nullptr);}
~RclCppFixture() {rclcpp::shutdown();}
};
RclCppFixture g_rclcppfixture;
class TestLifecycleNode : public nav2_util::LifecycleNode
{
public:
TestLifecycleNode()
: nav2_util::LifecycleNode("TestLifecycleNode")
{
}
nav2_util::CallbackReturn
on_configure(const rclcpp_lifecycle::State & state) override
{
robot_ = std::make_unique<nav2_robot::Robot>(shared_from_this());
robot_->on_configure(state);
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
on_activate(const rclcpp_lifecycle::State & state) override
{
robot_->on_activate(state);
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
on_deactivate(const rclcpp_lifecycle::State & state) override
{
robot_->on_deactivate(state);
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
on_cleanup(const rclcpp_lifecycle::State & state) override
{
robot_->on_cleanup(state);
robot_.reset();
return nav2_util::CallbackReturn::SUCCESS;
}
bool getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom)
{
return robot_->getOdometry(robot_odom);
}
std::string getName()
{
return robot_->getName();
}
bool getCurrentPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose)
{
return robot_->getCurrentPose(robot_pose);
}
void sendVelocity(geometry_msgs::msg::Twist twist)
{
robot_->sendVelocity(twist);
}
protected:
std::unique_ptr<nav2_robot::Robot> robot_;
};
class TestRobotClass : public ::testing::Test
{
public:
TestRobotClass()
{
// Create a lifecycle node that uses a robot and start a thread to handle its messages
lifecycle_node_ = std::make_shared<TestLifecycleNode>();
lifecycle_thread_ = std::make_unique<std::thread>(
[this](nav2_util::LifecycleNode::SharedPtr node) {
for (;; ) {
rclcpp::spin_some(node->get_node_base_interface());
if (shut_down_threads_) {return;}
}
}, lifecycle_node_
);
client_ = std::make_shared<rclcpp::Node>("test_robot_client_node");
lifecycle_client_ =
std::make_shared<nav2_util::LifecycleServiceClient>("TestLifecycleNode", client_);
lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE);
lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE);
// Initializing Pose and Twist messages
initTestPose();
initTestTwist();
// Creating fake publishers
pose_pub_ = client_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"amcl_pose", rclcpp::SystemDefaultsQoS().transient_local());
odom_pub_ = client_->create_publisher<nav_msgs::msg::Odometry>(
"odom", rclcpp::SystemDefaultsQoS());
// Subscribing to cmdVelocity topic to make sure Robot class is publishing velocity
vel_sub_ = client_->create_subscription<geometry_msgs::msg::Twist>("cmd_vel",
rclcpp::SystemDefaultsQoS(),
std::bind(&TestRobotClass::velocityReceived, this, std::placeholders::_1));
velocityCmdReceived_ = false;
}
~TestRobotClass()
{
lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
shut_down_threads_ = true;
lifecycle_thread_->join();
}
void velocityReceived(const geometry_msgs::msg::Twist::SharedPtr msg);
protected:
std::shared_ptr<rclcpp::Node> client_;
std::shared_ptr<TestLifecycleNode> lifecycle_node_;
std::unique_ptr<std::thread> lifecycle_thread_;
std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
geometry_msgs::msg::PoseWithCovarianceStamped testPose_;
geometry_msgs::msg::Twist testTwist_;
nav_msgs::msg::Odometry testOdom_;
geometry_msgs::msg::Twist velocityReceived_;
bool velocityCmdReceived_;
bool shut_down_threads_{false};
void initTestPose();
void initTestTwist();
void publishPose();
void publishOdom();
private:
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::ConstSharedPtr vel_sub_;
};
void
TestRobotClass::publishPose()
{
pose_pub_->publish(testPose_);
}
void
TestRobotClass::publishOdom()
{
testOdom_.pose = testPose_.pose;
testOdom_.twist.twist = testTwist_;
testOdom_.header.frame_id = "base_footprint";
testOdom_.header.stamp = rclcpp::Time();
odom_pub_->publish(testOdom_);
}
void
TestRobotClass::velocityReceived(const geometry_msgs::msg::Twist::SharedPtr msg)
{
velocityReceived_ = *msg;
velocityCmdReceived_ = true;
}
void TestRobotClass::initTestPose()
{
testPose_.header.frame_id = "testPose";
testPose_.header.stamp = rclcpp::Time();
testPose_.pose.pose.position.x = 0.0;
testPose_.pose.pose.position.y = 0.0;
testPose_.pose.pose.position.z = 0.0;
testPose_.pose.pose.orientation.x = 0.0;
testPose_.pose.pose.orientation.y = 0.0;
testPose_.pose.pose.orientation.z = 0.0;
testPose_.pose.pose.orientation.w = 1.0;
for (int i = 0; i < 12; i++) {
testPose_.pose.covariance[i] = 0.0;
}
}
void TestRobotClass::initTestTwist()
{
testTwist_.linear.x = 0.0;
testTwist_.linear.y = 0.0;
testTwist_.linear.z = 0.0;
testTwist_.angular.x = 0.0;
testTwist_.angular.y = 0.0;
testTwist_.angular.z = 0.0;
}
TEST_F(TestRobotClass, getNameTest)
{
std::string robotName = lifecycle_node_->getName();
EXPECT_EQ(robotName, "turtlebot");
}
TEST_F(TestRobotClass, getPoseTest)
{
auto currentPose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
while (!(lifecycle_node_->getCurrentPose(currentPose))) {
publishPose();
rclcpp::spin_some(client_);
}
EXPECT_EQ(*currentPose, testPose_);
}
TEST_F(TestRobotClass, getOdometryTest)
{
auto currentOdom = std::make_shared<nav_msgs::msg::Odometry>();
rclcpp::Rate r(10);
while (!(lifecycle_node_->getOdometry(currentOdom))) {
publishOdom();
r.sleep();
}
EXPECT_EQ(*currentOdom, testOdom_);
}
TEST_F(TestRobotClass, sendVelocityTest)
{
rclcpp::Rate r(10);
while (!velocityCmdReceived_) {
lifecycle_node_->sendVelocity(testTwist_);
rclcpp::spin_some(client_);
r.sleep();
}
EXPECT_EQ(testTwist_, velocityReceived_);
}
nav2_util/CMakeLists.txt
+ 0
− 16
View file @ 9534a7cd
......@@ -22,22 +22,6 @@ include_directories(include)
set(library_name ${PROJECT_NAME}_core)
add_subdirectory(src)
<<<<<<< HEAD
=======
install(TARGETS
${library_name}
map_lib
pf_lib
sensors_lib
motions_lib
map_loader
lifecycle_bringup
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
>>>>>>> resolve merge conflicts
install(DIRECTORY include/
DESTINATION include/
......
nav2_util/include/nav2_util/robot_utils.hpp
+ 10
− 6
View file @ 9534a7cd
......@@ -30,31 +30,35 @@ namespace nav2_util
class VelocityPublisher
{
public:
VelocityPublisher(rclcpp::Node::SharedPtr & node,
VelocityPublisher(
rclcpp::Node::SharedPtr & node,
const std::string topic = std::string("/cmd_vel"));
void publishCommand(const geometry_msgs::msg::Twist& cmd_vel);
void publishCommand(const geometry_msgs::msg::Twist & cmd_vel);
private:
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
};
class RobotStateHelper
{
public:
RobotStateHelper(rclcpp::Node::SharedPtr & node,
RobotStateHelper(
rclcpp::Node::SharedPtr & node,
const std::string odom_topic = std::string("/odom"));
bool getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom);
bool getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose,
bool getCurrentPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose,
const bool use_topic = true);
private:
void onPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr robot_pose);
void onOdomReceived(nav_msgs::msg::Odometry::SharedPtr msg);
bool getGlobalLocalizerPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
bool getGlobalLocalizerPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
bool getTfPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose);
private:
......
nav2_util/src/CMakeLists.txt
+ 0
− 10
View file @ 9534a7cd
......@@ -4,12 +4,8 @@ add_library(${library_name} SHARED
lifecycle_service_client.cpp
string_utils.cpp
lifecycle_utils.cpp
<<<<<<< HEAD
lifecycle_node.cpp
src/robot_utils.cpp
=======
robot_utils.cpp
>>>>>>> resolve merge conflicts
)
ament_target_dependencies(${library_name}
......@@ -20,10 +16,7 @@ ament_target_dependencies(${library_name}
nav_msgs
geometry_msgs
lifecycle_msgs
<<<<<<< HEAD
rclcpp_lifecycle
=======
>>>>>>> resolve merge conflicts
)
add_subdirectory(pf)
......@@ -36,7 +29,6 @@ add_executable(lifecycle_bringup
lifecycle_bringup_commandline.cpp
)
target_link_libraries(lifecycle_bringup ${library_name})
<<<<<<< HEAD
install(TARGETS
${library_name}
......@@ -45,5 +37,3 @@ install(TARGETS
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
=======
>>>>>>> resolve merge conflicts
nav2_util/src/map/CMakeLists.txt
+ 0
− 3
View file @ 9534a7cd
......@@ -5,7 +5,6 @@ add_library(map_lib SHARED
map_draw.c
map_cspace.cpp
)
<<<<<<< HEAD
install(TARGETS
map_lib
......@@ -13,5 +12,3 @@ install(TARGETS
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
=======
>>>>>>> resolve merge conflicts
nav2_util/src/map_loader/CMakeLists.txt
+ 0
− 3
View file @ 9534a7cd
......@@ -15,7 +15,6 @@ target_link_libraries(map_loader
${SDL_LIBRARY}
${SDL_IMAGE_LIBRARIES}
)
<<<<<<< HEAD
install(TARGETS
map_loader
......@@ -23,5 +22,3 @@ install(TARGETS
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
=======
>>>>>>> resolve merge conflicts
nav2_util/src/motion_model/CMakeLists.txt
+ 0
− 5
View file @ 9534a7cd
add_library(motions_lib SHARED
omni_motion_model.cpp
differential_motion_model.cpp
<<<<<<< HEAD
motion_model.cpp
)
target_link_libraries(motions_lib pf_lib)
......@@ -12,7 +11,3 @@ install(TARGETS
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
=======
)
target_link_libraries(motions_lib pf_lib)
>>>>>>> resolve merge conflicts
nav2_util/src/pf/CMakeLists.txt
+ 0
− 3
View file @ 9534a7cd
......@@ -10,7 +10,6 @@ add_library(pf_lib SHARED
eig3.c
pf_draw.c
)
<<<<<<< HEAD
install(TARGETS
pf_lib
......@@ -18,5 +17,3 @@ install(TARGETS
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
=======
>>>>>>> resolve merge conflicts
nav2_util/src/robot_utils.cpp
+ 32
− 16
View file @ 9534a7cd
......@@ -18,18 +18,24 @@
namespace nav2_util
{
VelocityPublisher::VelocityPublisher(rclcpp::Node::SharedPtr & node,
const std::string topic) {
vel_pub_ = node->create_publisher<geometry_msgs::msg::Twist>(topic, 1);
}
VelocityPublisher::VelocityPublisher(
rclcpp::Node::SharedPtr & node,
const std::string topic)
{
vel_pub_ = node->create_publisher<geometry_msgs::msg::Twist>(topic, 1);
}
void
VelocityPublisher::publishCommand(const geometry_msgs::msg::Twist& cmd_vel) {
vel_pub_->publish(cmd_vel);
void
VelocityPublisher::publishCommand(const geometry_msgs::msg::Twist & cmd_vel)
{
vel_pub_->publish(cmd_vel);
}
RobotStateHelper::RobotStateHelper(rclcpp::Node::SharedPtr & node,
const std::string odom_topic) : node_(node) {
RobotStateHelper::RobotStateHelper(
rclcpp::Node::SharedPtr & node,
const std::string odom_topic)
: node_(node)
{
pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"amcl_pose", std::bind(&RobotStateHelper::onPoseReceived, this, std::placeholders::_1));
......@@ -41,7 +47,8 @@ RobotStateHelper::RobotStateHelper(rclcpp::Node::SharedPtr & node,
}
bool
RobotStateHelper::getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom) {
RobotStateHelper::getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom)
{
std::shared_lock<std::shared_timed_mutex> lock(state_mutex_);
if (!initial_odom_received_) {
RCLCPP_DEBUG(node_->get_logger(),
......@@ -54,8 +61,10 @@ RobotStateHelper::getOdometry(nav_msgs::msg::Odometry::SharedPtr & robot_odom) {
}
bool
RobotStateHelper::getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose,
const bool use_topic) {
RobotStateHelper::getCurrentPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose,
const bool use_topic)
{
if (use_topic) {
return getGlobalLocalizerPose(robot_pose);
}
......@@ -64,7 +73,9 @@ RobotStateHelper::getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::
}
void
RobotStateHelper::onPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr robot_pose) {
RobotStateHelper::onPoseReceived(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr robot_pose)
{
std::unique_lock<std::shared_timed_mutex> lock(state_mutex_);
current_pose_ = robot_pose;
if (!initial_pose_received_) {
......@@ -73,7 +84,8 @@ RobotStateHelper::onPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::
}
void
RobotStateHelper::onOdomReceived(nav_msgs::msg::Odometry::SharedPtr msg) {
RobotStateHelper::onOdomReceived(nav_msgs::msg::Odometry::SharedPtr msg)
{
std::unique_lock<std::shared_timed_mutex> lock(state_mutex_);
current_odom_ = msg;
if (!initial_odom_received_) {
......@@ -82,7 +94,9 @@ RobotStateHelper::onOdomReceived(nav_msgs::msg::Odometry::SharedPtr msg) {
}
bool
RobotStateHelper::getGlobalLocalizerPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose) {
RobotStateHelper::getGlobalLocalizerPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose)
{
std::shared_lock<std::shared_timed_mutex> lock(state_mutex_);
if (!initial_pose_received_) {
RCLCPP_DEBUG(node_->get_logger(),
......@@ -95,7 +109,9 @@ RobotStateHelper::getGlobalLocalizerPose(geometry_msgs::msg::PoseWithCovarianceS
}
bool
RobotStateHelper::getTfPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & /*robot_pose*/) {
RobotStateHelper::getTfPose(
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & /*robot_pose*/)
{
std::shared_lock<std::shared_timed_mutex> lock(state_mutex_);
RCLCPP_DEBUG(node_->get_logger(), "getTfPose is not yet implemented.");
return false;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment