Skip to content

Commit 6136a8b

Browse files
SS47816SS47816
committed
feat: add goal_publisher.hpp
1 parent ec9ab7f commit 6136a8b

File tree

4 files changed

+101
-82
lines changed

4 files changed

+101
-82
lines changed

‎src/me5413_world/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ catkin_package(
2828
)
2929

3030
include_directories(
31-
# include
31+
include
3232
${catkin_INCLUDE_DIRS}
3333
)
3434

‎src/me5413_world/include/me5413_world/goal_publisher.hpp‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
/* goal_publisher.hpp
2+
3+
* Copyright (C) 2023 SS47816
4+
5+
* Declarations for GoalPublisherNode class
6+
7+
**/
8+
9+
#ifndef GOAL_PUBLISHER_H_
10+
#define GOAL_PUBLISHER_H_
11+
12+
#include <iostream>
13+
#include <string>
14+
#include <vector>
15+
16+
#include <ros/ros.h>
17+
#include <ros/console.h>
18+
#include <std_msgs/String.h>
19+
#include <std_msgs/Float32.h>
20+
#include <nav_msgs/Odometry.h>
21+
#include <geometry_msgs/Pose.h>
22+
#include <geometry_msgs/PoseStamped.h>
23+
#include <geometry_msgs/TransformStamped.h>
24+
25+
#include <tf2/convert.h>
26+
#include <tf2/LinearMath/Matrix3x3.h>
27+
#include <tf2/LinearMath/Transform.h>
28+
#include <tf2/LinearMath/Quaternion.h>
29+
#include <tf2_ros/transform_listener.h>
30+
#include <tf2_ros/transform_broadcaster.h>
31+
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
32+
33+
namespace me5413_world
34+
{
35+
36+
class GoalPublisherNode
37+
{
38+
public:
39+
GoalPublisherNode();
40+
virtual ~GoalPublisherNode() {};
41+
42+
private:
43+
void timerCallback(const ros::TimerEvent&);
44+
void robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom);
45+
void goalNameCallback(const std_msgs::String::ConstPtr& name);
46+
void goalPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& goal_pose);
47+
48+
tf2::Transform convertPoseToTransform(const geometry_msgs::Pose& pose);
49+
geometry_msgs::PoseStamped getGoalPoseFromConfig(const std::string& name);
50+
std::pair<double, double> calculatePoseError(const geometry_msgs::Pose& pose_robot, const geometry_msgs::Pose& pose_goal);
51+
52+
// ROS declaration
53+
ros::NodeHandle nh_;
54+
ros::Timer timer_;
55+
ros::Subscriber sub_robot_odom_;
56+
ros::Subscriber sub_goal_name_;
57+
ros::Subscriber sub_goal_pose_;
58+
ros::Publisher pub_goal_;
59+
ros::Publisher pub_absolute_position_error_;
60+
ros::Publisher pub_absolute_heading_error_;
61+
ros::Publisher pub_relative_position_error_;
62+
ros::Publisher pub_relative_heading_error_;
63+
tf2_ros::Buffer tf2_buffer_;
64+
tf2_ros::TransformListener tf2_listener_;
65+
tf2_ros::TransformBroadcaster tf2_bcaster_;
66+
// Robot pose
67+
std::string world_frame_;
68+
std::string map_frame_;
69+
std::string robot_frame_;
70+
geometry_msgs::Pose pose_world_robot_;
71+
geometry_msgs::Pose pose_world_goal_;
72+
geometry_msgs::Pose pose_map_robot_;
73+
geometry_msgs::Pose pose_map_goal_;
74+
std_msgs::Float32 absolute_position_error_;
75+
std_msgs::Float32 absolute_heading_error_;
76+
std_msgs::Float32 relative_position_error_;
77+
std_msgs::Float32 relative_heading_error_;
78+
};
79+
80+
} // namespace me5413_world
81+
82+
#endif // GOAL_PUBLISHER_H_

‎src/me5413_world/rviz/navigation.rviz‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -518,7 +518,7 @@ Visualization Manager:
518518
Enabled: true
519519
Name: ADE (m)
520520
Show Value: true
521-
Topic: /me5413_project/absolute/position_error
521+
Topic: /me5413_world/absolute/position_error
522522
Value: true
523523
auto color change: true
524524
auto scale: true
@@ -543,7 +543,7 @@ Visualization Manager:
543543
Enabled: true
544544
Name: AHE (deg)
545545
Show Value: true
546-
Topic: /me5413_project/absolute/heading_error
546+
Topic: /me5413_world/absolute/heading_error
547547
Value: true
548548
auto color change: true
549549
auto scale: true
@@ -566,7 +566,7 @@ Visualization Manager:
566566
- Class: jsk_rviz_plugin/PieChart
567567
Enabled: false
568568
Name: AHE (deg)
569-
Topic: /me5413_project/absolute/heading_error
569+
Topic: /me5413_world/absolute/heading_error
570570
Value: false
571571
auto color change: true
572572
background color: 0; 0; 0
@@ -595,7 +595,7 @@ Visualization Manager:
595595
Enabled: true
596596
Name: RDE (m)
597597
Show Value: true
598-
Topic: /me5413_project/relative/position_error
598+
Topic: /me5413_world/relative/position_error
599599
Value: true
600600
auto color change: true
601601
auto scale: true
@@ -620,7 +620,7 @@ Visualization Manager:
620620
Enabled: true
621621
Name: RHE (deg)
622622
Show Value: true
623-
Topic: /me5413_project/relative/heading_error
623+
Topic: /me5413_world/relative/heading_error
624624
Value: true
625625
auto color change: true
626626
auto scale: true
@@ -643,7 +643,7 @@ Visualization Manager:
643643
- Class: jsk_rviz_plugin/PieChart
644644
Enabled: false
645645
Name: RHE (deg)
646-
Topic: /me5413_project/relative/heading_error
646+
Topic: /me5413_world/relative/heading_error
647647
Value: false
648648
auto color change: true
649649
background color: 0; 0; 0

‎src/me5413_world/src/goal_publisher_node.cpp‎

Lines changed: 12 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -6,73 +6,10 @@
66
77
**/
88

9-
#include <iostream>
10-
#include <string>
11-
#include <vector>
12-
13-
#include <ros/ros.h>
14-
#include <ros/console.h>
15-
#include <std_msgs/String.h>
16-
#include <std_msgs/Float32.h>
17-
#include <nav_msgs/Odometry.h>
18-
#include <geometry_msgs/Pose.h>
19-
#include <geometry_msgs/PoseStamped.h>
20-
#include <geometry_msgs/TransformStamped.h>
21-
22-
#include <tf2/convert.h>
23-
#include <tf2/LinearMath/Matrix3x3.h>
24-
#include <tf2/LinearMath/Transform.h>
25-
#include <tf2/LinearMath/Quaternion.h>
26-
#include <tf2_ros/transform_listener.h>
27-
#include <tf2_ros/transform_broadcaster.h>
28-
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
29-
30-
namespace me5413_project
31-
{
9+
#include "me5413_world/goal_publisher.hpp"
3210

33-
class GoalPublisherNode
11+
namespace me5413_world
3412
{
35-
public:
36-
GoalPublisherNode();
37-
virtual ~GoalPublisherNode() {};
38-
39-
private:
40-
void timerCallback(const ros::TimerEvent&);
41-
void robotOdomCallback(const nav_msgs::Odometry::ConstPtr& odom);
42-
void goalNameCallback(const std_msgs::String::ConstPtr& name);
43-
void goalPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& goal_pose);
44-
45-
tf2::Transform convertPoseToTransform(const geometry_msgs::Pose& pose);
46-
geometry_msgs::PoseStamped getGoalPoseFromConfig(const std::string& name);
47-
std::pair<double, double> calculatePoseError(const geometry_msgs::Pose& pose_robot, const geometry_msgs::Pose& pose_goal);
48-
49-
// ROS declaration
50-
ros::NodeHandle nh_;
51-
ros::Timer timer_;
52-
ros::Subscriber sub_robot_odom_;
53-
ros::Subscriber sub_goal_name_;
54-
ros::Subscriber sub_goal_pose_;
55-
ros::Publisher pub_goal_;
56-
ros::Publisher pub_absolute_position_error_;
57-
ros::Publisher pub_absolute_heading_error_;
58-
ros::Publisher pub_relative_position_error_;
59-
ros::Publisher pub_relative_heading_error_;
60-
tf2_ros::Buffer tf2_buffer_;
61-
tf2_ros::TransformListener tf2_listener_;
62-
tf2_ros::TransformBroadcaster tf2_bcaster_;
63-
// Robot pose
64-
std::string world_frame_;
65-
std::string map_frame_;
66-
std::string robot_frame_;
67-
geometry_msgs::Pose pose_world_robot_;
68-
geometry_msgs::Pose pose_world_goal_;
69-
geometry_msgs::Pose pose_map_robot_;
70-
geometry_msgs::Pose pose_map_goal_;
71-
std_msgs::Float32 absolute_position_error_;
72-
std_msgs::Float32 absolute_heading_error_;
73-
std_msgs::Float32 relative_position_error_;
74-
std_msgs::Float32 relative_heading_error_;
75-
};
7613

7714
GoalPublisherNode::GoalPublisherNode() : tf2_listener_(tf2_buffer_)
7815
{
@@ -81,10 +18,10 @@ GoalPublisherNode::GoalPublisherNode() : tf2_listener_(tf2_buffer_)
8118
this->sub_goal_name_ = nh_.subscribe("/rviz_panel/goal_name", 1, &GoalPublisherNode::goalNameCallback, this);
8219
this->sub_goal_pose_ = nh_.subscribe("/move_base_simple/goal", 1, &GoalPublisherNode::goalPoseCallback, this);
8320
this->pub_goal_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);
84-
this->pub_absolute_position_error_ = nh_.advertise<std_msgs::Float32>("/me5413_project/absolute/position_error", 1);
85-
this->pub_absolute_heading_error_ = nh_.advertise<std_msgs::Float32>("/me5413_project/absolute/heading_error", 1);
86-
this->pub_relative_position_error_ = nh_.advertise<std_msgs::Float32>("/me5413_project/relative/position_error", 1);
87-
this->pub_relative_heading_error_ = nh_.advertise<std_msgs::Float32>("/me5413_project/relative/heading_error", 1);
21+
this->pub_absolute_position_error_ = nh_.advertise<std_msgs::Float32>("/me5413_world/absolute/position_error", 1);
22+
this->pub_absolute_heading_error_ = nh_.advertise<std_msgs::Float32>("/me5413_world/absolute/heading_error", 1);
23+
this->pub_relative_position_error_ = nh_.advertise<std_msgs::Float32>("/me5413_world/relative/position_error", 1);
24+
this->pub_relative_heading_error_ = nh_.advertise<std_msgs::Float32>("/me5413_world/relative/heading_error", 1);
8825

8926
// Initialization
9027
this->robot_frame_ = "base_link";
@@ -199,10 +136,10 @@ geometry_msgs::PoseStamped GoalPublisherNode::getGoalPoseFromConfig(const std::s
199136
*/
200137

201138
double x, y, yaw;
202-
nh_.getParam("/me5413_project" + name + "/x", x);
203-
nh_.getParam("/me5413_project" + name + "/y", y);
204-
nh_.getParam("/me5413_project" + name + "/yaw", yaw);
205-
nh_.getParam("/me5413_project/frame_id", this->world_frame_);
139+
nh_.getParam("/me5413_world" + name + "/x", x);
140+
nh_.getParam("/me5413_world" + name + "/y", y);
141+
nh_.getParam("/me5413_world" + name + "/yaw", yaw);
142+
nh_.getParam("/me5413_world/frame_id", this->world_frame_);
206143

207144
tf2::Quaternion q;
208145
q.setRPY(0, 0, yaw);
@@ -240,12 +177,12 @@ std::pair<double, double> GoalPublisherNode::calculatePoseError(const geometry_m
240177
return std::pair<double, double>(position_error, heading_error);
241178
}
242179

243-
} // namespace lgsvl_utils
180+
} // namespace me5413_world
244181

245182
int main(int argc, char** argv)
246183
{
247184
ros::init(argc, argv, "goal_publisher_node");
248-
me5413_project::GoalPublisherNode goal_publisher_node;
185+
me5413_world::GoalPublisherNode goal_publisher_node;
249186
ros::spin(); // spin the ros node.
250187
return 0;
251188
}

0 commit comments

Comments
 (0)