This page introduces the usage of set_initial_pose, including how to use 2D Pose Estimate in RVIZ to set the pose of the robot.

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 view_slamware_ros_sdk_server_node node. 
    roslaunch slamware_ros_sdk view_slamware_ros_sdk_server_node.launch
  8. Open another terminal, repeat step 5, and start the set_initial_pose_node node. 
    roslaunch slamware_ros_sample set_initial_pose.launch
  9. Click on '2D Pose Estimate' in RVIZ, then left-click on the desired location in the interface where you want to set the position of the car. While holding the left mouse button, move to change the angle. Once you release the mouse button, the pose is successfully set.
    In the image below, the left side shows the effect before setting, and the right side shows the effect after setting.

Code Description

Declare a global Publisher variable
ros::Publisher set_pose_pub;
Initialize the node and define publisher and subscriber variables
ros::init(argc, argv, "set_initial_pose_node");
ros::NodeHandle nh("~");
set_pose_pub = nh.advertise<geometry_msgs::Pose>("/slamware_ros_sdk_server_node/set_pose", 10);
ros::Subscriber initial_pose_sub = nh.subscribe("/initialpose", 10, initialPoseCallback);
Upon receiving the initialpose message, print out the message content, and then publish it
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped& msg)
{
  ROS_INFO("receive initialpose: ");
  ROS_INFO("position(xyz): %.3f %.3f %.3f", msg.pose.pose.position.x
                                          , msg.pose.pose.position.y
                                          , msg.pose.pose.position.z);
  ROS_INFO("orientation(xyzw): %.3f %.3f %.3f %.3f", msg.pose.pose.orientation.x
                                                    , msg.pose.pose.orientation.y
                                                    , msg.pose.pose.orientation.z
                                                    , msg.pose.pose.orientation.w);
  geometry_msgs::Pose _pose_msg(msg.pose.pose);
  set_pose_pub.publish(_pose_msg);
}


  • No labels