ROS 节点通过topic 或者service 控制节点启动关闭

最新推荐文章于 2025-05-14 09:29:55 发布

原创最新推荐文章于 2025-05-14 09:29:55 发布 · 5.2k 阅读

35 ·

CC 4.0 BY-SA版权

ROS机器人操作系统专栏收录该内容

21 篇文章

订阅专栏

需求在安卓端想通过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