Skip to content

Commit

Permalink
Revert some files
Browse files Browse the repository at this point in the history
  • Loading branch information
@nhatao
nhatao committed Jun 25, 2024
1 parent 3ad68f9 commit cd7027a
Show file tree
Hide file tree
Showing 5 changed files with 70 additions and 111 deletions.
45 changes: 22 additions & 23 deletions trajectory_tracker/src/trajectory_recorder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,13 @@
#include <cmath>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include <ros/ros.h>

#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <std_srvs/srv/empty.hpp>
#include <std_srvs/Empty.h>

#include <neonavigation_common/compatibility.h>

Expand All @@ -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_;
Expand All @@ -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()
Expand All @@ -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<nav_msgs::msg::Path>(
pub_path_ = neonavigation_common::compat::advertise<nav_msgs::Path>(
nh_, "path",
pnh_, topic_path_, 10, true);
srs_clear_path_ = pnh_.advertiseService("clear_path", &RecorderNode::clearPath, this);
Expand All @@ -99,41 +99,41 @@ 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;
}

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<tf2::Transform> 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);
Expand All @@ -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();
Expand Down
31 changes: 15 additions & 16 deletions trajectory_tracker/src/trajectory_saver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,10 +39,10 @@
#include <fstream>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include <ros/ros.h>

#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>

#include <neonavigation_common/compatibility.h>

Expand All @@ -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()
Expand All @@ -81,20 +81,20 @@ SaverNode::~SaverNode()
{
}

void SaverNode::cbPath(const nav_msgs::msg::Path::ConstSharedPtr& msg)
void SaverNode::cbPath(const nav_msgs::Path::ConstPtr& msg)
{
if (saved_)
return;
std::ofstream ofs(filename_.c_str());

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<uint8_t> buffer(new uint8_t[serial_size]);

ros::serialization::OStream stream(buffer.get(), serial_size);
Expand All @@ -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();
Expand Down
67 changes: 33 additions & 34 deletions trajectory_tracker/src/trajectory_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,15 @@
#include <fstream>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include <ros/ros.h>

#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/Twist.h>
#include <interactive_markers/interactive_marker_server.h>
#include <nav_msgs/msg/path.hpp>
#include <nav_msgs/Path.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <trajectory_tracker_msgs/srv/change_path.hpp>
#include <trajectory_tracker_msgs/msg/trajectory_server_status.hpp>
#include <visualization_msgs/msg/interactive_marker_update.hpp>
#include <trajectory_tracker_msgs/ChangePath.h>
#include <trajectory_tracker_msgs/TrajectoryServerStatus.h>
#include <visualization_msgs/InteractiveMarkerUpdate.h>

#include <trajectory_tracker/filter.h>

Expand All @@ -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<uint8_t> buffer_;
int serial_size_;
Expand All @@ -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
{
Expand All @@ -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<nav_msgs::msg::Path>(
pub_path_ = neonavigation_common::compat::advertise<nav_msgs::Path>(
nh_, "path",
pnh_, topic_path_, 2, true);
pub_status_ = pnh_.advertise<trajectory_tracker_msgs::msg::TrajectoryServerStatus>("status", 2);
pub_status_ = pnh_.advertise<trajectory_tracker_msgs::TrajectoryServerStatus>("status", 2);
srv_change_path_ = neonavigation_common::compat::advertiseService(
nh_, "change_path",
pnh_, "ChangePath", &ServerNode::change, this);
Expand Down Expand Up @@ -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:
Expand All @@ -166,18 +166,18 @@ 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";
srv_im_fb_.clear();
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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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();
Expand Down
2 changes: 0 additions & 2 deletions trajectory_tracker/src/trajectory_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -354,7 +354,6 @@ void TrackerNode::cbTimer()
try
{
tf2::Stamped<tf2::Transform> 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_);
}
Expand Down Expand Up @@ -509,7 +508,6 @@ TrackerNode::TrackingResult TrackerNode::getTrackingResult(const tf2::Stamped<tf
try
{
tf2::Stamped<tf2::Transform> 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;
Expand Down
Loading

0 comments on commit cd7027a

Please sign in to comment.