diff --git a/trajectory_tracker/src/trajectory_recorder.cpp b/trajectory_tracker/src/trajectory_recorder.cpp index ee1d2391..729b3e98 100644 --- a/trajectory_tracker/src/trajectory_recorder.cpp +++ b/trajectory_tracker/src/trajectory_recorder.cpp @@ -38,13 +38,13 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include -#include -#include +#include +#include #include #include -#include +#include #include @@ -56,8 +56,8 @@ class RecorderNode void spin(); private: - bool clearPath(std_srvs::srv::Empty::Request& req, - std_srvs::srv::Empty::Response& res); + bool clearPath(std_srvs::Empty::Request& req, + std_srvs::Empty::Response& res); std::string topic_path_; std::string frame_robot_; @@ -66,14 +66,14 @@ class RecorderNode double ang_interval_; bool store_time_; - rclcpp::Node nh_; - rclcpp::Node pnh_; + ros::NodeHandle nh_; + ros::NodeHandle pnh_; ros::Publisher pub_path_; tf2_ros::Buffer tfbuf_; tf2_ros::TransformListener tfl_; ros::ServiceServer srs_clear_path_; - nav_msgs::msg::Path path_; + nav_msgs::Path path_; }; RecorderNode::RecorderNode() @@ -89,7 +89,7 @@ RecorderNode::RecorderNode() pnh_.param("ang_interval", ang_interval_, 1.0); pnh_.param("store_time", store_time_, false); - pub_path_ = neonavigation_common::compat::advertise( + pub_path_ = neonavigation_common::compat::advertise( nh_, "path", pnh_, topic_path_, 10, true); srs_clear_path_ = pnh_.advertiseService("clear_path", &RecorderNode::clearPath, this); @@ -99,13 +99,13 @@ RecorderNode::~RecorderNode() { } -float dist2d(geometry_msgs::msg::Point& a, geometry_msgs::msg::Point& b) +float dist2d(geometry_msgs::Point& a, geometry_msgs::Point& b) { return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); } -bool RecorderNode::clearPath(std_srvs::srv::Empty::Request& /* req */, - std_srvs::srv::Empty::Response& /* res */) +bool RecorderNode::clearPath(std_srvs::Empty::Request& /* req */, + std_srvs::Empty::Response& /* res */) { path_.poses.clear(); return true; @@ -113,27 +113,27 @@ bool RecorderNode::clearPath(std_srvs::srv::Empty::Request& /* req */, void RecorderNode::spin() { - rclcpp::Rate loop_rate(50); + ros::Rate loop_rate(50); path_.header.frame_id = frame_global_; path_.header.seq = 0; - while (rclcpp::ok()) + while (ros::ok()) { - rclcpp::Time now = rclcpp::Time(0); + ros::Time now = ros::Time(0); if (store_time_) - now = rclcpp::Time::now(); + now = ros::Time::now(); tf2::Stamped transform; try { tf2::fromMsg( - tfbuf_.lookupTransform(frame_global_, frame_robot_, now, rclcpp::Duration(0.2)), transform); + tfbuf_.lookupTransform(frame_global_, frame_robot_, now, ros::Duration(0.2)), transform); } catch (tf2::TransformException& e) { - RCLCPP_WARN(rclcpp::get_logger("TrajectoryTracker"), "TF exception: %s", e.what()); + ROS_WARN("TF exception: %s", e.what()); continue; } - geometry_msgs::msg::PoseStamped pose; + geometry_msgs::PoseStamped pose; tf2::Quaternion q; transform.getBasis().getRotation(q); pose.pose.orientation = tf2::toMsg(q); @@ -159,15 +159,14 @@ void RecorderNode::spin() pub_path_.publish(path_); } - rclcpp::spin_some(node); + ros::spinOnce(); loop_rate.sleep(); } } int main(int argc, char** argv) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("trajectory_recorder"); + ros::init(argc, argv, "trajectory_recorder"); RecorderNode rec; rec.spin(); diff --git a/trajectory_tracker/src/trajectory_saver.cpp b/trajectory_tracker/src/trajectory_saver.cpp index 62d2bb63..6fbf8844 100644 --- a/trajectory_tracker/src/trajectory_saver.cpp +++ b/trajectory_tracker/src/trajectory_saver.cpp @@ -39,10 +39,10 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include -#include -#include +#include +#include #include @@ -54,14 +54,14 @@ class SaverNode void save(); private: - rclcpp::Node nh_; - rclcpp::Node pnh_; + ros::NodeHandle nh_; + ros::NodeHandle pnh_; ros::Subscriber sub_path_; std::string topic_path_; std::string filename_; bool saved_; - void cbPath(const nav_msgs::msg::Path::ConstSharedPtr& msg); + void cbPath(const nav_msgs::Path::ConstPtr& msg); }; SaverNode::SaverNode() @@ -81,7 +81,7 @@ SaverNode::~SaverNode() { } -void SaverNode::cbPath(const nav_msgs::msg::Path::ConstSharedPtr& msg) +void SaverNode::cbPath(const nav_msgs::Path::ConstPtr& msg) { if (saved_) return; @@ -89,12 +89,12 @@ void SaverNode::cbPath(const nav_msgs::msg::Path::ConstSharedPtr& msg) if (!ofs) { - RCLCPP_ERROR(rclcpp::get_logger("TrajectoryTracker"), "Failed to open %s", filename_.c_str()); + ROS_ERROR("Failed to open %s", filename_.c_str()); return; } uint32_t serial_size = ros::serialization::serializationLength(*msg); - RCLCPP_INFO(rclcpp::get_logger("TrajectoryTracker"), "Size: %d\n", (int)serial_size); + ROS_INFO("Size: %d\n", (int)serial_size); boost::shared_array buffer(new uint8_t[serial_size]); ros::serialization::OStream stream(buffer.get(), serial_size); @@ -107,23 +107,22 @@ void SaverNode::cbPath(const nav_msgs::msg::Path::ConstSharedPtr& msg) void SaverNode::save() { - rclcpp::Rate loop_rate(5); - RCLCPP_INFO(rclcpp::get_logger("TrajectoryTracker"), "Waiting for the path"); + ros::Rate loop_rate(5); + ROS_INFO("Waiting for the path"); - while (rclcpp::ok()) + while (ros::ok()) { - rclcpp::spin_some(node); + ros::spinOnce(); loop_rate.sleep(); if (saved_) break; } - RCLCPP_INFO(rclcpp::get_logger("TrajectoryTracker"), "Path saved"); + ROS_INFO("Path saved"); } int main(int argc, char** argv) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("trajectory_saver"); + ros::init(argc, argv, "trajectory_saver"); SaverNode rec; rec.save(); diff --git a/trajectory_tracker/src/trajectory_server.cpp b/trajectory_tracker/src/trajectory_server.cpp index 3e7f44c2..c3c2632a 100644 --- a/trajectory_tracker/src/trajectory_server.cpp +++ b/trajectory_tracker/src/trajectory_server.cpp @@ -39,15 +39,15 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include -#include +#include #include -#include +#include #include -#include -#include -#include +#include +#include +#include #include @@ -61,16 +61,16 @@ class ServerNode void spin(); private: - rclcpp::Node nh_; - rclcpp::Node pnh_; + ros::NodeHandle nh_; + ros::NodeHandle pnh_; ros::Publisher pub_path_; ros::Publisher pub_status_; ros::ServiceServer srv_change_path_; interactive_markers::InteractiveMarkerServer srv_im_fb_; - nav_msgs::msg::Path path_; + nav_msgs::Path path_; std::string topic_path_; - trajectory_tracker_msgs::srv::ChangePath::Request req_path_; + trajectory_tracker_msgs::ChangePath::Request req_path_; double hz_; boost::shared_array buffer_; int serial_size_; @@ -79,10 +79,10 @@ class ServerNode bool loadFile(); void loadPath(); - bool change(trajectory_tracker_msgs::srv::ChangePath::Request& req, - trajectory_tracker_msgs::srv::ChangePath::Response& res); + bool change(trajectory_tracker_msgs::ChangePath::Request& req, + trajectory_tracker_msgs::ChangePath::Response& res); void processFeedback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); void updateIM(); enum { @@ -105,10 +105,10 @@ ServerNode::ServerNode() pnh_.param("hz", hz_, 5.0); pnh_.param("filter_step", filter_step_, 0.0); - pub_path_ = neonavigation_common::compat::advertise( + pub_path_ = neonavigation_common::compat::advertise( nh_, "path", pnh_, topic_path_, 2, true); - pub_status_ = pnh_.advertise("status", 2); + pub_status_ = pnh_.advertise("status", 2); srv_change_path_ = neonavigation_common::compat::advertiseService( nh_, "change_path", pnh_, "ChangePath", &ServerNode::change, this); @@ -136,19 +136,19 @@ bool ServerNode::loadFile() } void ServerNode::processFeedback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback) + const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { int id = std::atoi(feedback->marker_name.c_str()); switch (feedback->event_type) { - case visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE: + case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE: path_.poses[id].pose = feedback->pose; break; - case visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP: - path_.header.stamp = rclcpp::Time::now(); + case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP: + path_.header.stamp = ros::Time::now(); pub_path_.publish(path_); break; - case visualization_msgs::msg::InteractiveMarkerFeedback::MENU_SELECT: + case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT: switch (feedback->menu_entry_id) { case MENU_DELETE: @@ -166,7 +166,7 @@ void ServerNode::processFeedback( void ServerNode::updateIM() { - visualization_msgs::msg::InteractiveMarkerUpdate viz; + visualization_msgs::InteractiveMarkerUpdate viz; viz.type = viz.KEEP_ALIVE; viz.seq_num = update_num_++; viz.server_id = "Path"; @@ -174,10 +174,10 @@ void ServerNode::updateIM() int i = 0; for (auto& p : path_.poses) { - visualization_msgs::msg::InteractiveMarker mark; - visualization_msgs::msg::Marker marker; - visualization_msgs::msg::InteractiveMarkerControl ctl; - visualization_msgs::msg::MenuEntry menu; + visualization_msgs::InteractiveMarker mark; + visualization_msgs::Marker marker; + visualization_msgs::InteractiveMarkerControl ctl; + visualization_msgs::MenuEntry menu; mark.header = path_.header; mark.pose = p.pose; mark.scale = 1.0; @@ -232,8 +232,8 @@ void ServerNode::updateIM() srv_im_fb_.applyChanges(); } -bool ServerNode::change(trajectory_tracker_msgs::srv::ChangePath::Request& req, - trajectory_tracker_msgs::srv::ChangePath::Response& res) +bool ServerNode::change(trajectory_tracker_msgs::ChangePath::Request& req, + trajectory_tracker_msgs::ChangePath::Response& res) { req_path_ = req; res.success = false; @@ -243,7 +243,7 @@ bool ServerNode::change(trajectory_tracker_msgs::srv::ChangePath::Request& req, res.success = true; ros::serialization::IStream stream(buffer_.get(), serial_size_); ros::serialization::deserialize(stream, path_); - path_.header.stamp = rclcpp::Time::now(); + path_.header.stamp = ros::Time::now(); if (filter_step_ > 0) { std::cout << filter_step_ << std::endl; @@ -277,24 +277,23 @@ bool ServerNode::change(trajectory_tracker_msgs::srv::ChangePath::Request& req, void ServerNode::spin() { - rclcpp::Rate loop_rate(hz_); - trajectory_tracker_msgs::msg::TrajectoryServerStatus status; + ros::Rate loop_rate(hz_); + trajectory_tracker_msgs::TrajectoryServerStatus status; - while (rclcpp::ok()) + while (ros::ok()) { status.header = path_.header; status.filename = req_path_.filename; status.id = req_path_.id; pub_status_.publish(status); - rclcpp::spin_some(node); + ros::spinOnce(); loop_rate.sleep(); } } int main(int argc, char** argv) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("trajectory_server"); + ros::init(argc, argv, "trajectory_server"); ServerNode serv; serv.spin(); diff --git a/trajectory_tracker/src/trajectory_tracker.cpp b/trajectory_tracker/src/trajectory_tracker.cpp index d36ff22b..c9ca7b21 100644 --- a/trajectory_tracker/src/trajectory_tracker.cpp +++ b/trajectory_tracker/src/trajectory_tracker.cpp @@ -354,7 +354,6 @@ void TrackerNode::cbTimer() try { tf2::Stamped transform; - // tf2::fromMsg(tfbuf_.lookupTransform(frame_robot_, frame_odom_, tf2::TimePointZero), transform); tf2::fromMsg(tfbuf_.lookupTransform(frame_robot_, frame_odom_, rclcpp::Time(0)), transform); control(transform, Eigen::Vector3d(0, 0, 0), 0, 0, 1.0 / hz_); } @@ -509,7 +508,6 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult(const tf2::Stamped odom_to_path; - // tf2::fromMsg(tfbuf_.lookupTransform(frame_odom_, path_header_.frame_id, tf2::TimePointZero), odom_to_path); tf2::fromMsg(tfbuf_.lookupTransform(frame_odom_, path_header_.frame_id, rclcpp::Time(0)), odom_to_path); transform *= odom_to_path; diff --git a/trajectory_tracker/test/src/test_sample.cpp b/trajectory_tracker/test/src/test_sample.cpp deleted file mode 100644 index 9c1493f3..00000000 --- a/trajectory_tracker/test/src/test_sample.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// test_sample.cpp -#include -#include - -class SomethingTest : public ::testing::Test -{ -protected: - void SetUp() override - { - _node = rclcpp::Node::make_shared("test_node"); - RCLCPP_INFO(_node->get_logger(), "SetUp %x, %x", _node.get(), this); - x_ = 5; - } - - int x_; - rclcpp::Node::SharedPtr _node; -}; - -TEST_F(SomethingTest, Add) -{ - EXPECT_EQ(10, x_ + 5); -} - -TEST_F(SomethingTest, Add2) -{ - EXPECT_EQ(10, x_ + 6); -} - -int main(int argc, char** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} \ No newline at end of file