ros_notes_new - RicoJia/notes GitHub Wiki

##ROS Foundamentals---------------------------

1. simple service client

    ros::ServiceClient set_pose_srv_client;
    set_pose_srv_client = nh.serviceClient<turtlesim::TeleportAbsolute>("/Odometer/set_pose");
    ros::service::waitForService("/Odometer/set_pose", 10);
    turtlesim::TeleportAbsolute srv;
    srv.request.theta = 0.0;
    srv.request.x =  wp_vec[0].x;
    srv.request.y =  wp_vec[0].y;
    if (set_pose_srv_client.call(srv)) {
    ROS_INFO("Odometer Pose has been reset successfully!");
} else {
    ROS_WARN("WARNING: Odometer Pose failed to be reset!");
}

• Note: this also works with dynamic_reconfigure service calls, such as /set_parameters. You just need client_.call(req,res);

4. If you need an "universal service", that is, not depending on another package for building its srv file, then you might need to consider dynamic_reconfigure. Method 1: global calls local 1, local 1 will call planner 1 - potential asynchronization issue Method 2: global calls planner 1, but this wouldn't work Method 3: lcoal planner calls global planner, upon getting a new plan. Global plan should have /get_params, then call the secondary planner param. Not the most straight-forward way but should work. (no service called get_params!!) Method 4: setting parameter server. global will change its private parameter: /move_base/GP/speed_limit -> fastrack base local planner.

5. Nuances

• If there is no callback: service call will be pushed to the server queue -> will have to wait forever for the server to respond, and block the client node from proceeding (blocking) -> it will return once the call is done (but that never happens in this case).

• When does a service call fail? service fails when you didn't waitforservice, or have connection failure or return false (so the response will not be sent to the caller), not when connection succeeds and has to wait for ros::spin call.. If the service call succeeded, call() will return true and the value in srv.response will be valid. If the call did not succeed, call() will return false and the value in srv.response will be invalid.

1. MessageEvent - Used to retrieve metadata, such as publisher node name, time of receipt. e.g,

   void callback(const ros::MessageEvent<std_msgs::String const>& event)
   {
     const std::string& publisher_name = event.getPublisherName();
     ros::Time receipt_time = event.getReceiptTime();
     const std_msgs::StringConstPtr& msg = event.getMessage();
   }

2. echo

   • echo once: rostopic echo /topic -n1

3. ptr, constptr ...

void someCb(const std_msgs::String::ConstPtr& msg);     //ConstPtr, Ptr are generated by message_generation. They are typedefs for smart pointers. 

• Intraprocess message passing: If you launch publishes & subscribes to the same topic in the same node (like a nodelet manager), just this boost::shared_ptr is being passed, and no serialization, deserialization is involved. If in different nodes, copies are still made for serialization (deserialization: a message is received -> gets pushed onto queue -> gets deserialized for the first callback which requires copying. ). It's only mostly safe if the ptr is not modified after reception.

http://wiki.ros.org/roscpp/Internals

4. Be careful with having custom messages with the same name built in different workspaces - they might be able to override one another!! 【坑】

5. Great Tool: rosmsg show ... so you can see the current definition of a message.

6. KEY TAKEAWAY: after you build a message, you should remove /build, /devel, and build it immediately!!

1. name space: usually given in launch file.

• ros::NodeHandle nh("name") gives /name_space/name
• ros::NodeHandle nh2(nh1, "nh2") gives /name_space/name/nh2.
• ros::NodeHandle global_nh("/global") gives /global, and cannot be given a namespace in launch file.
• ros::NodeHandle private_nh("~private") gives /node_name/private.
• ros::NodeHandle private_nh("~"); gives /node_name

2. remapping from command-line rosrun package node /name:=/some_name (so this will remap /name regardless of the namespace. Otherwise, node_name space will be applied.)
   • changing the namespace of a node rosrun turtlesim turtle_teleop_key __ns:=ajsim
   • you can also do: __master:=
3. Assign a private parameter: rosrun rospy_tutorials talker _param:=1.0
4. when the node reads params, the param should have the private name space. Read below for ways to set that.
5. In code, you can Check the name of the node by: ROS_INFO("Name is %s", ros::this_node::getName());

1. add a library

   catkin_package(LIBRARIES ${PROJECT_NAME})
   add_library(moo src/moo.cpp)
   target_link_libraries(foo moo)  -- This links foo against libmoo.so
   install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
   

2. Use a library

   find_package(LIB_NAME)
   catkin_package()
   #add_dependencies(node ${rigid2d_EXPORTED_TARGETS}) # this is for using actions, services, etc. from another package. Your library should not contain any ROS libraries. 
   target_link_libraries(node LIB_NAME)
   

   In package.xml,

   <depend>LIB_NAME</depend>       
   

   Note: if your LIB_NAME is not a ros package, rosdep will have a hard time finding it!
3. rosdep rosdep install --from-paths src --ignore-src -r -y

1. If your target depends on another target, which needs messages, services to be built, include add_dependencies(some_target ${catkin_EXPORTED_TARGETS}), if your target uses messages generated by your own package, have this add_dependencies(some_target ${${PROJECT_NAME}_EXPORTED_TARGETS}) Therefore, your best bet is ** add_dependencies(some_target ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})**

```
find_package (Eigen3 REQUIRED)
catkin_package(DEPENDS Eigen3)
```
in hpp
```
#include <Eigen/Dense>
```

• nodelet_manager: node where a nodelet is dynamically loaded. It's listens to ros services, they can be embedded within running nodes. A nodelet will contact the nodelet manager to instantiate an instance of a nodelet.

###TF

1. listen to TF rostopic echo tf: that is slow! rosrun tf tf_echo parent_frame child_frame

$rosrun tf view_frames
$ evince frames.pdf

or rosrun rqt_tf_tree rqt_tf_tree

1. GOOD PRACTICE: USE TRY-EXCEPT. Because tf will throw away frames if they come in too far apart. Try again on your next iterations.

• Use rospy.time() (most recent time?) instead of rospy.time.now() (the current time)

2. ROS MELODIC has tf2, but kinetic does not!!

3. ros::Time::now() vs ros::Time(0) for TF - always use ros::Time(0) as it means "the latest". Every TF has a buffer, so when TF gets into a buffer from a broadcaster, it takes time.

4. Example try{ listener.waitForTransform(TARGET_FRAME, SOURCE_FRAME, ros::Time(0), ros::Duration(0.4)); //latest transform, to express SOURCE_FRAME IN TARGET FRAME, (T_target_source)e, with duration 0.4s. so this is Tsource-> target. listener.lookupTransfrom(TARGET_FRAME, SOURCE_FRAME, ros::Time(0), transform);
   }

5. waitForTransform is blocking, until a valid transform comes in.

1. tf::StampedTransform -> tf::Transform, it is a child class, so you have all the Transform APIs.
2. geometry_msgs::TransformStamped to tf::Transform

   static void tf::transformStampedMsgToTF 	( 	const geometry_msgs::TransformStamped &  	msg, StampedTransform & bt)
   

###TF2

1. tf and tf2

• tf is used on tf2, so tf2 is newer, but many concepts are the same
• Most of tf is in tf2, except tf_conversions.transformations. tf2 uses external library transformations3d, which is NOT packaged in ROS>
• TF 2 can allow you to keep track of transforms for up to 10s, using a buffer

2. Documentation

• tf2 Design Guide explains the rationale for tf2
• tf2_ros Documentation provides a guide to the API The generated python API docs are broken.
• If you download the source code (https://github.com/ros/geometry2) and look in the python files in the tf2_ros/src/tf2_ros directory, comments in the code describe the API
• The python API parallels the C++ API so the C++ API docs are also useful
• In particular, the python code for the tf2_ros is a useful resource.
• It is also possible to generate the documentation locally using rosdoc_lite.
• The transformations under tf_conversions.transformations are documented here.
• It is also useful to read the source code to gain a better understanding of how it works.

1. create a listener

   tf2_ros::Buffer tf_buffer;  
   tf2_ros::TransformListener tf_listener (tf_buffer); //make sure this tf_listener is persistent, like making it a member variable. 

2.look up a transform cpp world_to_robot_tf_ = tf_buffer.lookupTransform(TARGET_FRAME, SOURCE_FRAME, ros::Time(0)); //Don't we need to wait? 3. tf2 conversions: cpp tf2::doTransform (const geometry_msgs::PoseStamped &t_in, geometry_msgs::PoseStamped &t_out, const geometry_msgs::TransformStamped &transform) Note: doTransform can automatically check if t_in and t_out have the same frame_ids as transform. If t_in is already in the target frame, t_out will be copied. 4. tf::Transform Inversion tf::Transform::inverse()

• geometry_msgs::TransfromStamped
  • geometry_msgs::Transform geometry_msgs/Vector3 translation x,y,z geometry_msgs/Quaternion rotation, x,y,z,w
  • geometry_msgs::pose: geometry_msgs/Point position x,y,z geometry_msgs/Quaternion orientation, x,y,z,w
• tf2::Transfrom //#include <tf2/LinearMath/Transform.h> If you cannot find the h file, go search the github!!
  • tf2::Vector3 getOrigin(),
    m_origin.x(); m_origin.y(); m_origin.z();
  • tf2::Matrix3x3 getBasis() //#include <tf2/LinearMath/Matrix3x3.h> tf2::Vector3() getRow(int i)
  • tf2::Quaternion getRotation() tf2::tf2Scalar getAngle() tf2::Vector getAxis()
• conversions
  • geometry_msgs::Transform -> tf2::Transform

  #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
      geometry_msgs::Transform tf_g = tf2::toMsg(const tf2::Transform& T)

1. launch-prefix: can enable xterm, gdb, etc. You can use an arg for simpler debug mode switch

   • xterm: default terminal emulator for linux system running on X Window. You computer runs on GNOME and does not have xterm by default. once you hit xterm, it's going to give you X11 environment and a terminal window.
   • it can be "" for nothing.
   • anon: launch a node anonymously. It generates an anonymous id based on name.

2. passing args

• This is for basic types: str, bool, int, float

      <node pkg="rospy_tutorials" type="talker" name="talker">
       <!-- set a private parameter for the node -->
       <param name="talker_1_param" value="a value" />        
        <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
        <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
      <.node>
  

• This is for more complicated types: in YAML

  <rosparam param="end_effector_initial_xyz">
      {"x":0.0, "y":0.0, "z":0.42}
  </rosparam>
  

  In yaml, you can use the keyword pi
• 【tricky】a way to pass arguments in xacro:

   <arg name="cmd" default="$(find xacro)/xacro --inorder $(find omnid)/urdf/delta_robot.urdf.xacro
     R_base:=0.18
     h_base:=0.009525
    "/>
  <param name="robot_description" command="$(arg cmd)"/> 
  

3. Substitute in an environment variable value

4. <param> in included files will be travesed depth-first style. The last setting will win - the inner most setting will win. This is not recommended as the param name in a file might change. So use arg tag.

   • <param> can have type inside. you can specify boolean.
   • loading a yaml file to the current node namespace, under <node> tag , in code use ros::NodeHandle (~)

5. all tags have <if>. Inside <node>, you cannot have a group tag, but can have <if>.

6. set groups if/unless args

<arg name="robot" default='-1' doc =''\>
<group if='$(eval arg('robot')!=-1)'>
<group unless='$(eval arg('robot')!=-1)'>    

it's equivalent to have, so eval needs to be followed by an argument

"$(eval arg('foo'))"
"$(eval foo)"

8. remap a topic (naming)

   <node pkg="turtlesim" name="mimic" type="mimic">
    <remap from="input" to="turtlesim1/turtle1"/>	#change the node name of the topic to /turtlesim1/turtle1/topic_name
    <remap from="output" to="turtlesim2/turtle1"/>	# change the topic you publish. 
   </node>

1. yaml cares about spacing.
2. extra namespace can be added in the form of yaml dictionary: /rosparam_namespace/LOL block style

LOL: 
    lol: "loll"
    lol2: "lolll"

is equivalent to flow style: LOL:{lol:"loll", lol2"lolll"}

In yaml, there is no distinction bw private and public parameters. private parameters is just name resolution that adds extra name space to the variables. When you do ("~"), you are actually exapnding ~ into the current name space of your node.

3. The way to launch a vector of things, You don't know the vector size!!

plugins:
    - {name: obstacles, type: "costmap_2d::ObstacleLayer"}

00114   if (private_nh.hasParam("plugins"))
00115   {
00116     XmlRpc::XmlRpcValue my_list;
00117     private_nh.getParam("plugins", my_list);
00118     for (int32_t i = 0; i < my_list.size(); ++i)
00119     {
00120       std::string pname = static_cast<std::string>(my_list[i]["name"]);
00127     }
00128   } 

nh.hasParam("lol2");

1. You can publish a topic to invoke the action serverros_.

0. Intro

   • Pluginlib will tell the build system about this plugin registration. Later, the plugin user can call "rospack" (tool to see live package info) to see which plugins are available under parent_class.
   • then the plugin user can use pluginlib::class_loader to create an instance of the package.

1. Basic structure: C++ class (class funcs and registration) 2. Cmake, package.xml 3. extra xml file 4. Use plugin.

2. class funcs: a. Build a base class, and child classes b. you need default constructor; you also need initialize 【重要】Registration: - Pay attention to the class qualifiers. a. in .cpp file, need PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon)

3. Cmake
   include_directories(include) add_library(polygon_plugins src/polygon_plugins.cpp)

package.xml: (only one , registering the plugin in the ROS Package System) a. <pluginlib_tutorials_ plugin="${prefix}/polygon_plugins.xml" /> pluginlib_tutorials_ is the pacakage of the base_class lives.

   【重要】b. Child class package.xml
     <build_depend>polygon_interface</build_depend>
     <run_depend>polygon_interface</run_depend> 

4. xml - to make it visible to ros tool chain a. should be with CMakelist.txt and package.xml b. library path: relative path to a library we want to export (in devel/lib) c. list all classes d. Following naming conventions: base_local_planner is blp.xml, base_global_planner is bgp.xml!!

5. Verification

   1. see plugin has been built to ros tool chain: - output path to polygon_plugins.xml rospack plugins --attrib=plugin pluginlib_tutorials_ 【技巧】Can be used to see all classes associated.
   2. run the executable $./build/robot_arm_ik_plugin/polygon_loader

6. Use the plug-in

• The ClassLoader must not go out scope while you are using the plugin. So, make class loader a member variable of that class.
• How to create an instance:

performance_local_planner_loader_ = pluginlib::ClassLoader<nav_core::BaseLocalPlanner> ("nav_core", "nav_core::BaseLocalPlanner")
performance_local_planner_ = performance_local_planner_loader_.createInstance(plp_name_);
//this plp_name is the name in the xml file of the plugin. But the real name can be different.  

• How to know the actual name of the plugin?
  performance_local_planner_loader_.getName(plp_name_)

0. package.xml:

  <depend>dynamic_reconfigure</depend>

1. cfg/ClassName.cfg

# Forearm camera configuration
PACKAGE='package_name'
import roslib; roslib.load_manifest(PACKAGE)
from dynamic_reconfigure.parameter_generator import *
gen = ParameterGenerator()
angles = gen.add_group("Angles")
#       Name                    Type      Reconfiguration level
#       Description
#       Default  Min  Max
angles.add("min_ang",           double_t, SensorLevels.RECONFIGURE_STOP,
           "The angle of the first range measurement. The unit depends on ~ang_radians.",
           -pi/2,-pi, pi)
gen.add("skip",                 int_t,    SensorLevels.RECONFIGURE_STOP,
        "The number of scans to skip between each measured scan",
         0, 0,  9)
gen.add("port",                 str_t,    SensorLevels.RECONFIGURE_CLOSE,
        "The serial port where the hokuyo device can be found",
        "/dev/ttyACM0")
gen.add("calibrate_time",       bool_t,   SensorLevels.RECONFIGURE_CLOSE,
        "Whether the node should calibrate the hokuyo's time offset",
         True)
exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "ClassName"))

Then make it executablechmod a+x your_package/cfg/ClassName.cfg

2. cpp file

#include <dynamic_reconfigure/server.h>
#include <your_package/MyStuffConfig.h>

void callback(your_package::MyStuffConfig &config, uint32_t level)
{
           config.groups.angles.min_ang;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "dynamic_reconfigure_node");
  dynamic_reconfigure::Server<your_package::MyStuffConfig> srv;
  dynamic_reconfigure::Server<your_package::MyStuffConfig>::CallbackType f;
  f = boost::bind(&callback, _1, _2);
  srv.setCallback(f);
  ROS_INFO("Starting to spin...");
  ros::spin();
  return 0;
}

3. CMakelist.xtx

 . find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure ) 
 . generate_dynamic_reconfigure_options( cfg/MyStuff.cfg)  
 . include_directories(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) 
 . add_executable(dynamic_reconfigure_node src/dynamic_reconfigure_node.cpp)
 . add_dependencies(dynamic_reconfigure_node ${PROJECT_NAME}_gencfg)
 . target_link_libraries(dynamic_reconfigure_node ${catkin_LIBRARIES})

1. service client cpp

#include <dynamic_reconfigure/DoubleParameter.h>    // ROS already has it. 
#include <dynamic_reconfigure/Reconfigure.h>
#include <dynamic_reconfigure/Config.h> 

void ApproachMarker::EnableArTrackAlvar(bool enable) {
  dynamic_reconfigure::ReconfigureRequest srv_req;
  dynamic_reconfigure::ReconfigureResponse srv_resp;
  dynamic_reconfigure::BoolParameter enable_param;  //or dynamic_reconfigure::DoubleParameter double_param; 
  dynamic_reconfigure::Config conf;

  enable_param.name = "enabled";    //same for double_param
  enable_param.value = enable;
  conf.bools.push_back(enable_param);

  srv_req.config = conf;

  if (ros::service::call("/ar_track_alvar/set_parameters", srv_req, srv_resp)) {
    ROS_INFO("call to set ar_track_alvar parameters succeeded");
  } else {
    ROS_INFO("call to set ar_track_alvar parameters failed");
  }
2. Server: 
Do not forget to 

}

1. Several Boost library is already built into Linux (usr/local). So check the current version of boost, and if you need to sue a newer version of Boost, you should do find_package(Boost REQUIRED 1.65 COMPONENTS ..). This doesn't have to be built separately. (boost::shared_ptr does not even need to be included)

2. However, boost libraries need to be found, here is how

1. use case: when you have a small code, and there's ros::NodeHandle related object, you cannot define them right on the spot: because global variables are defined before main(), but your object depends on main. So, you use a boost::Shared_pointer.

boost::shared_pointer<tf::TransformListener> tf_ptr;
int main(int argc, char** argv){
    ros::init(argc, argv, "my_handle"); 
    tf_ptr.reset(new tf::TransformListener); 
}

2. boost::shared_ptr vs std::shared_ptr

• boost::shared_ptr is used more widely as it came about earlier than std::shared_ptr.
• Many message ptrs, such as std_msgs::StringPtr, are redefinitions of boost::shared_ptr:

typedef boost::shared_ptr< ::std_msgs::String> std_msgs::StringPtr

• This is useful in intraprocess messaging, i.e, you have a subscriber and a publisher to the same topic in a single node. You publish the sharedptr, and the subscriber will get it. Ofc, you don't wanna change the sharedptr after it is received.

If you wanna send different info, you need to create a new ptr. - Nodelets are using this.

3. ROS API has boost::shared_ptr<>, (as a default? ) but ROS2 has modern C++, with std::shared_ptr, not boost::shared_ptr. As a result, you should also use boost::dynamic_cast

4. boost::dynamic_ptr_cast

allows to have nodes (3D point cloud, etc. ) to run multithreaded.

1. Here I assume std::thread and std::scoped_lock will achieve the same thing as their boost equivalent

boost::mutex obj_mutex
boost::thread t1(obj.function);
boost::thread t2(obj.function);

obj_type::function{
    boost::mutex::scoped_lock(obj_mutex); 
}

How to create a thread multiple ways

    void FastrackBaseGlobalPlanner::makePlanSlowPlannerWrapper(boost::promise<bool> &p){
        p.set_value(true);
        ROS_ERROR("DELETE THIS: DONE SLOW PLANNING");
    }
    
    void another_func(){
        bool slow_planner_succ;
        boost::promise<bool> slow_planner_p;
        boost::unique_future<bool> slow_planner_f = slow_planner_p.get_future();
        boost::thread slow_planner_t{&FastrackBaseGlobalPlanner::makePlanSlowPlannerWrapper, this, std::ref(slow_planner_p)}; 
        slow_planner_succ = slow_planner_f.get();
    }

Even tho ROScpp subscribers are executed in single thread, if you have a large function where variables might change by subscribers, you want to lock the variables. So in this case, those subscriber callbacks will try to update the variable After acquiring the same lock object. If they cannot acquire the lock, they will be spinning.

1. use key_teleop node, which gives you linear and angular velocities.

1. Do not call costmap2d ros when initializing the global planner, because:

//move_base.cpp:  
plugin-initialization, move_base
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();

costmap will be initialized (including (dynamic reconfigure service)drsv and its callback), but paused. In the code, there is no spin. So the service caller will wait for spin forever, so this will hang.

1. #ifndef: checks if something has been defined in the file, or in any included file. If not, define a variable: #ifndef #define #endif
2. When you have output arguments:

int exampleMethod(FooThing input, BarThing* output);

use pointer instead of reference, because the reader has to read the funtion prototype to see if a variable can be modified.
3. ROS Wrapper A design patter that: instantiate an object (like costmap_2d), reads parameters from the parameter server, and subscribes to/ publishes on topics by namespace. For example costmap2d_ros:

Level 1 - a. YAML (dictionary form), launched under a node b. Node: other services, costmap_ = new costmap_2d::Costmap2DROS(COSTMAP_NAME, tf);

Level 2 - http://docs.ros.org/jade/api/costmap_2d/html/costmap__2d__ros_8h_source.html
Costmap2d_ROS (wrapper)
Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) //load parameters using XmlRpc list; 
**boost::shared_ptr<base_type> plugin_ptr = plugin_loader_.createInstance(type)   //use plugin
plugin -> initialize(); SUPER COMMON TO HAVE THIS PTR!!** 

Level 3 - http://docs.ros.org/jade/api/costmap_2d/html/classcostmap__2d_1_1Layer.html#a8238b7e03dc09bade9543a23addc65e2
BaseType(){some_base_variables; onInitialize} 

Level 4 - http://docs.ros.org/jade/api/costmap_2d/html/classcostmap__2d_1_1ObstacleLayer.html
ChildClass(){onInitialize(){...}}

1. Docker issues

2. After catkin_make, docker does not give you the most updated version. - source it again? Nope - Did you put the wrong debugging message in the wrong place?? Likely.

3. after rosdep, you run catkin build, but there's a long error message suggesting different alternatives, exit the docker and come back in.

4. I see parameters when the ros node is not running?

   • The built-in parameter server should not persist after ROS Master is shut down. This parameter is just a runtime dictionary. And has not been optimzed. But I can still see it?