This page introduces the usage of sync_get_stcm, including how to retrieve the STCM map through the sync_get_stcm service.


Contents


Environment Setup

software platform

  • Ubuntu 20.04 X86
  • ROS Noetic

hardware platform

Choose any one below:

  • Slamware Kit (User Robot System based on Slamware Solution)
  • robot base systems like Apollo/Ares/Athena 

Sample Code Download

ROS-Sample Download Link


Compile and Run

  1. Download the Slamware ROS SDK along with ROS examples.
  2. Place the 'src' folder from the ROS SDK into an empty working directory, such as 'catkin_ws'. Also, put the 'slamware_ros_sample' folder from the ROS examples into the 'src' folder.
    mkdir -p catkin_ws/src
    cd catkin_ws/src
  3. Initialize the workspace using the Catkin tool.
    catkin_init_workspace
  4. Compile.
    cd ..
    catkin_make
  5. Set up workspace environment.
    source devel/setup.bash

  6. Launch the slamware_ros_sdk_server_node node. Select the corresponding IP according to the mode.
    roslaunch slamware_ros_sdk slamware_ros_sdk_server_node.launch ip_address:=10.6.128.141    // Station mode
    // If in AP mode, the IP address is 192.168.11.1
  7. Open another terminal, repeat step 5, and start the sync_get_stcm_node node.
    roslaunch slamware_ros_sample sync_get_stcm.launch
    // In the launch file, there is a default definition for the map_file_name parameter, indicating the path and filename where the map is saved. you can adjust parameters manually.
  8. At this point, observe under slamware_ros_sample, a folder named 'map' will be generated, containing a file named 'data.map'.

Code Description

sync_get_stcm
int main (int argc, char **argv)
{
  std::string map_file_name;

  ros::init(argc, argv, "sync_set_stcm_node");
  ros::NodeHandle nh("~");
  ros::ServiceClient client = nh.serviceClient<slamware_ros_sdk::SyncGetStcm>("/slamware_ros_sdk_server_node/sync_get_stcm");
  slamware_ros_sdk::SyncGetStcm srv;

  //get file name from paramter server  
	if (!nh.getParam("map_file_name",map_file_name))
	{
		ROS_ERROR("invalied file path......");
		return 1;
	}
  
  //call service
  if (client.call(srv))
  {
    ROS_INFO("get raw_stcm data success,writing to file......");
    
    //check whether the target folder exists. create one if not
    std::string dir;
    dir = map_file_name.substr(0,map_file_name.rfind("/"));
    if(access(dir.c_str(),0))
    {
      std::string cmd = "mkdir -p " + dir;
      system(cmd.c_str());
    }

		std::ofstream fout(map_file_name, std::ios::out | std::ios::binary);
		fout.write(reinterpret_cast<char *>(srv.response.raw_stcm.data()),(srv.response.raw_stcm.size()) * sizeof(srv.response.raw_stcm.front()));
		fout.close();
    
        ROS_INFO("raw_stcm data size: %zu",srv.response.raw_stcm.size());
		ROS_INFO("save map at %s success......",map_file_name.c_str());
  }
  else
  {
    ROS_ERROR("Failed to get raw_stcm data......");
    return 1;
  }

    return 0;
}


  • No labels