diff --git a/.travis.yml b/.travis.yml index 74cf56ed580..fc79bb3cca0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -26,5 +26,5 @@ before_install: script: - docker run --name nav2_bash --rm -d -i -t nav2:latest bash -l - docker exec nav2_bash bash -c '. /ros2_ws/navigation2_ws/install/setup.bash && - colcon test --packages-skip nav2_tasks' + colcon test --ctest-args " -V" --packages-skip nav2_tasks' - docker exec nav2_bash bash -c "colcon test-result --verbose" diff --git a/nav2_robot/include/nav2_robot/robot.hpp b/nav2_robot/include/nav2_robot/robot.hpp index d8b1e993cb9..1ef0aab7379 100644 --- a/nav2_robot/include/nav2_robot/robot.hpp +++ b/nav2_robot/include/nav2_robot/robot.hpp @@ -31,8 +31,11 @@ class Robot explicit Robot(rclcpp::Node::SharedPtr & node); Robot() = delete; - bool getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose); - bool getCurrentVelocity(nav_msgs::msg::Odometry::SharedPtr & robot_velocity); + 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); @@ -52,8 +55,8 @@ class Robot // The current pose as received from the Pose subscription geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose_; - // The current velocity as received from the Odometry subscription - nav_msgs::msg::Odometry::SharedPtr current_velocity_; + // 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_; diff --git a/nav2_robot/src/robot.cpp b/nav2_robot/src/robot.cpp index 90315c4d3c9..fa1482496c5 100644 --- a/nav2_robot/src/robot.cpp +++ b/nav2_robot/src/robot.cpp @@ -47,14 +47,15 @@ Robot::onPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::Share void Robot::onOdomReceived(const nav_msgs::msg::Odometry::SharedPtr msg) { - current_velocity_ = msg; + current_odom_ = msg; if (!initial_odom_received_) { initial_odom_received_ = true; } } bool -Robot::getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose) +Robot::getGlobalLocalizerPose( + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose) { if (!initial_pose_received_) { RCLCPP_WARN(node_->get_logger(), @@ -66,8 +67,16 @@ Robot::getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & return true; } +// TODO(mhpanah): We should get current pose from transforms. bool -Robot::getCurrentVelocity(nav_msgs::msg::Odometry::SharedPtr & robot_velocity) +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_WARN(node_->get_logger(), @@ -75,7 +84,7 @@ Robot::getCurrentVelocity(nav_msgs::msg::Odometry::SharedPtr & robot_velocity) return false; } - robot_velocity = current_velocity_; + robot_odom = current_odom_; return true; } diff --git a/nav2_robot/test/test_robot_class.cpp b/nav2_robot/test/test_robot_class.cpp index 3049873d0bc..aac8e08c0b8 100644 --- a/nav2_robot/test/test_robot_class.cpp +++ b/nav2_robot/test/test_robot_class.cpp @@ -141,11 +141,11 @@ TEST_F(TestRobotClass, getPoseTest) EXPECT_EQ(*currentPose, testPose_); } -TEST_F(TestRobotClass, getVelocityTest) +TEST_F(TestRobotClass, getOdometryTest) { auto currentOdom = std::make_shared(); rclcpp::Rate r(10); - while (!(robot_->getCurrentVelocity(currentOdom))) { + while (!(robot_->getOdometry(currentOdom))) { publishOdom(); rclcpp::spin_some(node_); r.sleep();