需求 在安卓端想通過topic 或者 service 控制某人節點的啓動 關閉
代碼
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <stdio.h>
using namespace std;
int statue_flag = 0;
bool exit_flag = false;
void HandleCmdCallback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I herad :[%s]",msg->data.c_str());
/*
if(msg->data == "start")
{
ROS_INFO("start");
system("/home/tank/carto030_ws/src/tank_app/sh/start_cartographer.sh");
ROS_INFO("start end");
}
if(msg->data == "close")
{
ROS_INFO("close");
system("/home/tank/carto030_ws/src/tank_app/sh/shut_cartographer.sh");
}
*/
if(msg->data == "start")
{
if(statue_flag != 0)
{
statue_flag = 0;
::ros::shutdown();
}
else
{
cout << "statue_flag err "<<endl;
}
}
if(msg->data == "close")
{
if(statue_flag != 1)
{
::ros::shutdown();
statue_flag = 1;
}
else
{
cout << "statue_flag err "<<endl;
}
}
if(msg->data == "shutdown")
{
exit_flag = true;
ROS_INFO("shutdown");
::ros::shutdown();
}
}
void start_cartographer(int argc, char** argv)
{
ros::init(argc, argv, "cartographer_node");
ros::NodeHandle nh;
ROS_INFO("cartographer_node");
ros::Subscriber sub = nh.subscribe("cartographer_topic",1000,HandleCmdCallback);
//::ros::start();
ros::spin();
ROS_INFO("shutdown");
cout<<("cartographer_node shutdown\n");
}
void manage_node(int argc, char** argv)
{
ros::init(argc, argv, "tank_app");
ros::NodeHandle nh;
ROS_INFO("manage_node");
ros::Subscriber sub = nh.subscribe("tank_control",1000,HandleCmdCallback);
ros::spin();
ROS_INFO("shutdown");
cout<<("manerage end\n")<<endl;
}
int main(int argc, char** argv)
{
int loop_cnt =0;
//for(loop_cnt = 0; loop_cnt < 3; loop_cnt++)
while(exit_flag == false)
{
cout<<"loop_cnt"<< loop_cnt<<endl;
switch(statue_flag)
{
case 0:
start_cartographer(argc, argv);
statue_flag = 1;
break;
case 1:
manage_node(argc, argv);
statue_flag = 0;
// sleep(10);
break;
default:
break;
}
loop_cnt++;
while(ros::ok())
{
sleep(10);
cout<<"--wait for shutdown"<<endl;
}
{
cout<<"--shutdown ok"<<endl;
}
}
ROS_INFO("exit exit");
#if 0
loop:
ros::init(argc, argv, "tank_app2s");
ros::NodeHandle nh;
ros::Subscriber sub2 = nh.subscribe("tank_control_XXX",1000,HandleCmdCallback);
ros::Rate loop_rate(10);
while(ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
goto loop;
#endif
return(0);
}
實例 安卓通過topic service 控制 cartographer_ros 建圖啓動與關閉node_main.c
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"
#include <map>
#include <string>
#include "cartographer/io/proto_stream.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/mapping/proto/pose_graph.pb.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/proto/submap.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/submaps.h"
#include "cartographer_ros/ros_map.h"
#include "cartographer_ros/submap.h"
#include "glog/logging.h"
#include "std_msgs/String.h"
DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, "
"second is always the Cartographer installation to allow "
"including files from there.");
DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the "
"configuration file.");
DEFINE_string(map_filename, "", "If non-empty, filename of a map to load.");
DEFINE_bool(
start_trajectory_with_default_topics, true,
"Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
save_map_filename, "",
"If non-empty, serialize state and write it to disk before shutting down.");
namespace cartographer_ros {
namespace {
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 1e6;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(tf_buffer);
NodeOptions node_options;
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
Node node(node_options, &tf_buffer);
if (!FLAGS_map_filename.empty()) {
LOG(ERROR)<<"-----------------LoadMap() FLAGS_map_filename:"<<FLAGS_map_filename;
node.LoadMap(FLAGS_map_filename);
}
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
//savemap_thread_ = new boost::thread(boost::bind(&Node::OptimizationAndSaveMap, this));
::ros::spin();
LOG(ERROR)<<"-------::ros::spin();end------";
node.FinishAllTrajectories();
node.RunFinalOptimization();
if (!FLAGS_save_map_filename.empty()) {
//LOG(ERROR)<<"-------FLAGS_save_map_filename------mapname"<<FLAGS_save_map_filename;
//node.SerializeState(FLAGS_save_map_filename);
}
//std::string map_path_name = "/home/tank/map/map_data.pbstream";
//node.SerializeState(map_path_name);
node.save_to_ros_map();
LOG(ERROR)<<"-------exite run------";
}
} // namespace
} // namespace cartographer_ros
#if 0
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
/*
CHECK(!FLAGS_save_map_pgm.empty())
<< "-save_map_pgm is missing.";
*/
::ros::init(argc, argv, "cartographer_node");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run();
::ros::shutdown();
}
#else
int statue_flag = 1;
bool exit_flag = false;
void HandleCmdCallback(const std_msgs::String::ConstPtr& msg)
{
LOG(INFO)<<"I herad :[%s]"<<msg->data.c_str();
/*
if(msg->data == "start")
{
ROS_INFO("start");
system("/home/tank/carto030_ws/src/tank_app/sh/start_cartographer.sh");
ROS_INFO("start end");
}
if(msg->data == "close")
{
ROS_INFO("close");
system("/home/tank/carto030_ws/src/tank_app/sh/shut_cartographer.sh");
}
*/
if(msg->data == "start")
{
if(statue_flag != 0)
{
statue_flag = 0;
::ros::shutdown();
}
else
{
ROS_INFO("statue_flag err ");
}
}
if(msg->data == "close")
{
if(statue_flag != 1)
{
::ros::shutdown();
statue_flag = 1;
}
else
{
ROS_INFO("statue_flag err ");
}
}
if(msg->data == "shutdown")
{
exit_flag = true;
ROS_INFO("shutdown\n");
::ros::shutdown();
}
}
void cartographer_node(int argc, char** argv)
{
LOG(ERROR)<<"************entry cartographer_node *******************";
::ros::init(argc, argv, "cartographer_node");
::ros::NodeHandle nh_xx;
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
ROS_INFO("manage_node");
::ros::Subscriber sub = nh_xx.subscribe("tank_control_c",1000,HandleCmdCallback);
cartographer_ros::Run();
LOG(ERROR)<<"------- exite cartographer_node ------";
// ::ros::shutdown();
}
void manager_node(int argc, char** argv)
{
LOG(ERROR)<<"進入 : manager_node ";
::ros::init(argc, argv, "manager_node");
::ros::NodeHandle nh_xx;;
//::ros::start();
::ros::Subscriber sub = nh_xx.subscribe("tank_control_m",1000,HandleCmdCallback);
::ros::spin();
//cartographer_ros::Run();
//::ros::shutdown();
LOG(ERROR)<<"退出 : manager_node ";
}
int main(int argc, char** argv)
{
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
int loop_cnt = 0;
//for(loop_cnt = 0; loop_cnt < 3; loop_cnt++)
while(exit_flag == false)
{
LOG(ERROR)<<"--------loop_cnt: "<< loop_cnt;
switch(statue_flag)
{
case 0:
cartographer_node(argc, argv);
statue_flag = 1;
break;
case 1:
manager_node(argc, argv);
statue_flag = 0;
// sleep(10);
break;
default:
break;
}
loop_cnt++;
while(::ros::ok())
{
sleep(1);
LOG(ERROR)<<("--wait for shutdown");
}
{
sleep(1);
LOG(ERROR)<<("--shutdown ok");
}
}
return 0;
ROS_INFO("exit exit");
}
#endif