Fork: 0
tsukuba2022 / waypoint_navigation
Browse code
update
master
1 parent d562270 commit a9e06ab67699df00a4c489a718933a4d82c169f9
@koki koki authored on 15 Aug 2022
Patch
Unified Split
Showing 2 changed files
View
8
■■■■■
 waypoint_nav/param/test.yaml 100644 → 0
waypoints:
- point: {x: 0, y: 0, z: 0, vel: 1, rad: 0.3}
finish_pose:
header: {seq: 0, stamp: 0.000000000, frame_id: }
pose:
position: {x: 0, y: 0, z: 0}
orientation: {x: 0, y: 0, z: 0, w: 0}
View
71
■■■■■
 waypoint_saver/src/waypoint_saver.cpp
 
class WaypointsSaver{
public:
WaypointsSaver() :
filename_("waypoints.yaml")
filename_("waypoints.yaml"),
default_rad_(0.8)
{
waypoints_viz_sub_ = nh_.subscribe("waypoints_viz", 1, &WaypointsSaver::waypointsVizCallback, this);
waypoints_joy_sub_ = nh_.subscribe("waypoints_joy", 1, &WaypointsSaver::waypointsJoyCallback, this);
finish_pose_sub_ = nh_.subscribe("finish_pose", 1, &WaypointsSaver::finishPoseCallback, this);
private_nh.param("filename", filename_, filename_);
private_nh.param("save_joy_button", save_joy_button_, 0);
private_nh.param("robot_frame", robot_frame_, std::string("base_link"));
private_nh.param("world_frame", world_frame_, std::string("map"));
private_nh.param("default_rad", default_rad_, 0.8);
}
void waypointsJoyCallback(const sensor_msgs::Joy &msg) {
private_nh.param("default_rad", default_rad_, default_rad_);
}
void waypointsJoyCallback(const sensor_msgs::Joy &msg)
{
static ros::Time saved_time(0.0);
//ROS_INFO_STREAM("joy = " << msg);
if(msg.buttons[save_joy_button_] == 1 && (ros::Time::now() - saved_time).toSec() > 3.0){
if(msg.buttons[save_joy_button_] == 1 && (ros::Time::now() - saved_time).toSec() > 3.0) {
tf::StampedTransform robot_gl;
try{
try {
tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
Waypoint point;
point.set_x(robot_gl.getOrigin().x()); // point.point.x = robot_gl.getOrigin().x();
point.set_y(robot_gl.getOrigin().y()); //point.point.y = robot_gl.getOrigin().y();
point.set_z(robot_gl.getOrigin().z()); //point.point.z = robot_gl.getOrigin().z();
point.set_x(robot_gl.getOrigin().x());
point.set_y(robot_gl.getOrigin().y());
point.set_z(robot_gl.getOrigin().z());
point.set_rad(default_rad_);
waypoints_.push_back(point);
saved_time = ros::Time::now();
}catch(tf::TransformException &e){
}
catch(tf::TransformException &e) {
ROS_WARN_STREAM("tf::TransformException: " << e.what());
}
}
}
void waypointsVizCallback(const geometry_msgs::PointStamped &msg) {
void waypointsVizCallback(const geometry_msgs::PointStamped &msg)
{
Waypoint point;
point.set_x(msg.point.x);
point.set_y(msg.point.y);
point.set_z(msg.point.z);
point.set_rad(default_rad_);
ROS_INFO_STREAM("point = " << point);
waypoints_.push_back(point);
}
void finishPoseCallback(const geometry_msgs::PoseStamped &msg) {
void finishPoseCallback(const geometry_msgs::PoseStamped &msg)
{
finish_pose_ = msg;
save2();
waypoints_.clear();
}
void save() {
void save()
{
std::ofstream ofs(filename_.c_str(), std::ios::out);
ofs << "waypoints:" << std::endl;
for(int i=0; i < waypoints_.size(); i++){
for(int i=0; i < waypoints_.size(); i++) {
ofs << " " << "- point:" << std::endl;
ofs << " x: " << waypoints_[i].point.x << std::endl;
ofs << " y: " << waypoints_[i].point.y << std::endl;
ofs << " z: " << waypoints_[i].point.z << std::endl;
ROS_INFO_STREAM("write success");
}
 
 
void save2() {
Waypoint point;
point.set_x(0.0);
point.set_y(0.0);
point.set_z(0.0);
waypoints_.push_back(point);
 
void save2()
{
std::ofstream ofs(filename_.c_str(), std::ios::out);
/*
waypoints:
- point: {x: *, y: *, z: *, vel: *, rad: *}
ROS_INFO_STREAM("write success");
}
 
void run() {
void run()
{
ros::spin();
}
 
 
};
 
 
 
int main(int argc, char *argv[]) {
int main(int argc, char *argv[])
{
ros::init(argc, argv, "waypoints_saver");
WaypointsSaver saver;
saver.save2();
saver.run();