ROS_Utils - RicoJia/notes GitHub Wiki

========================================================================

ROS log

========================================================================

  1. rosout is the console reporting mechanism. It is: a rosout node, /rosout topic, GUI tools, like rqt_console.

    • rosout also uses rosconsole package which provides interface to roscpp, stream like or printf like.
      1. ROS_INFO("lol %s", "hello"); // printf style.
    • all rosconsole APIs are like ROS_

  2. 5 levels of base debug messages (all macros, transfers a stringstream into logging system, log4xx, stringstream and the log4xx is destroyed when this process is completed).

    • ROS_DEBUG
    • ROS_INFO
    • ROS_WARN
    • ROS_ERROR
    • ROS_FATAL

========================================================================

ROS time

========================================================================

  1. timer

    • The purpose is to call a callback at a fixed rate. It is not for hard timing purposes.
    • ros::Time::now() returns 0 until the first /clock msg is received. So use a loop for that
    • timer callback, same callback mechanism as subscriber, so basically single-threaded.

  2. toSec: double secs =ros::Time::now().toSec();

========================================================================

ROS Control

========================================================================

  1. Idea: robot needs commanded angles, but also need to publish joint_states: Link

    • ROS Control will provide such a "hardware interface", and PID controllers to interface with that
    • there're joint position, joint effort controllers.
    • On real robot, construct two elements for hardware interface to call: jnt_state_interface, jnt_pos_interface. jnt_state_interface will record commands (effort, position, whatever)

  2. Set up ros control on robot

    • controller.yaml - For gazebo, like electritian work

    • config for starting ros control and launch file

    • Input to the whole system will be joint position/velocity ...

    • Different controllers

========================================================================

URDF

========================================================================

Basics

  1. In General, ROS has multiple ways to express angles, but in URDF, it's ZYX Euler Angles. See here

STL

  • https://www.youtube.com/watch?v=r-
  • note mesh has to be in this way: 
    meshfile="package://rjje_arm/meshes/box.stl"
    • mesh 
      <xacro:macro name="m_link_mesh" params="name origin_xyz origin_rpy meshfile meshscale mass ixx ixy ixz iyy iyz izz">
  <link name="${name}">
    <inertial>
      <mass value="${mass}" />
      <origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
      <inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}" iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
    </inertial>
    <collision>
      <origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
      <geometry>
        <mesh filename="${meshfile}" scale="${meshscale}"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
      <geometry>
        <mesh filename="${meshfile}" scale="${meshscale}"/>
      </geometry>
      <material name="light_black"/>
    </visual>
  </link>
</xacro:macro>
  1. Example
  2. Joint origin, by default is the same as child frame, and its origin is relative to the last joint!!. You can move the child frame away from the joint frame, but that doesn't change the next joint's relative position to the parent one.
  3. urdf does not have variables

========================================================================

Xacro

========================================================================

Basics

  1. Xacro file:
    1. it's an xml file that automates writing URDF file (most robots use Xacro, not URDF directly). (most robots use Xacro, not URDF directly)
    2. It can use simple math (read using python and that library is already loaded.).
    3. It has Xacro Macro (functions), which allows you to create instances instead of writing everything out in URDF (Create two identical arms)
    4. Then, it can be converted to URDF, and can be loaded onto robot_description in launch file. This conversion is automatic.

Read a xacro file

  1. read xacro file for rvizLaunch file you need:

    <param name="robot_description"
command="$(find xacro)/xacro $(find mypackage)/urdf/myrobot.urdf.xacro"/>

  2. Convert Xacro in CMakelist.txt at compile time:

    1. find package: find_package(xacro REQUIRED)
    2. xacro_add_files(myxacrofiles TARGET xacro_files)
    3. modify URDF if you need to.

  3. load URDF into Rviz

    <!-- load the urdf into the parameter server -->
<param name="robot_description" textfile="$(find turtle_circle)/urdf/urdf_arm.urdf" />
<!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_arm)/rviz/view_robot.rviz"/>    -->
<node name="rviz" pkg="rviz" type="rviz"/>

  4. visualizing xacro in Rviz thru URDF:

    • We need a base link in the URDF. However if we have a mobile robot, what's the relationship between base link and the robot?
    • Answer: just use odom frame. the plug in does that for you: transform b/w fixed frame and base link

  5. mimic_joint: mimic_joint: value = multiplier * other_joint_value + offset.

    <joint name="robot_${prefix}/mimic_theta_${id}" type="revolute">
  <parent link="robot_${prefix}/mimic_lower_leg_${id}" />
  <child link="robot_${prefix}/mimic_theta_link_${id}" />
  <origin xyz="0 0 0" rpy="0 0 0" />
  <axis xyz = "0 -1 0" rpy="0 0 0"/>
  <limit lower="${-pi/2.0}" upper="${pi/2.0}" effort="100" velocity="100" />
  <dynamics damping="0.0" friction="0.0"/>
  <mimic joint="robot_${prefix}/theta_${id}" multiplier="-1" offset="0"/>
</joint>

Write a xacro file

  1. How to write xacro file

    • macro language for XML, you need to chmod 755 a xacro file to make it an executable. Xacro property can be inserted anywhere
    • include yaml file:<xacro:property name="yaml_file" value="$(find ddrive)/config/ddrive_params.yaml" />
    • include another xacro file: <xacro:include filename="$(find mrm_description)/urdf/robot_parameters.xacro" />
    • Replace a variable in Xacro <xacro:property name = "name" value = "value"> to define a constant in xacro
    • Basic constants (like pi) are already defined, You can use any python expression anywhere using the ${} syntax
    • It often makes sense to load non-changeable robot parameters directly from a yaml file 
      <xacro:property name="yaml_file"    value="$(find package)/config/config.yaml" />
<xacro:property name="properties"   value="${load_yaml(yaml_file)}" />

<xacro:property name="property"  value="${properties['property']}" /> 
<xacro:property name="synergy_weight" value="${synergies[finger][joint]}"/>
    • radius="${2*radius}" #you can use math here.
    • 大坑　it's ${2*radius}, not ${radius}*2
    • Debugging: macros can be nested. add macros slowly. view them in URDF step by step
    • 大坑 xacro file 中若要用不同的if condition, 你一定要改掉相应的tag。 比如material tag 要用不同的name。 不然会出问题！！
    • 大坑 xacro 中， 你的macro Name 不能是joint或者link！

  2. conditional block

    <xacro:if value="<expression>">
<... some xml code here ...>
</xacro:if>
<xacro:unless value="<expression>">
<... some xml code here ...>
</xacro:unless>

  3. Small Syntaxes

    • You can even specify if, else in ${PYTHON_CODE_HERE}: origin xyz="${0 if axis == 'z' else h/2.0}

========================================================================

Rviz

========================================================================

Basics

  1. rviz by default will give you x,y,z view

    rosrun rviz rviz
    • Loading views: need .rviz file in rviz directory for 
      <node name="rviz" pkg="rviz" type="rviz" args="-d $(find prm)/config/visualize_prm.rviz" required="true"/>

  2. joint_state_publisher: should run JSP, with ~use_gui = true, so you can change JSP to see how it moves!

    1. information flow:

      message topics on the source list (different sensors), or robot_description from URDF file
->Joint state publisher (JSP) 
-> joint_state topic (sensor_msgs/Jointstate).
-> robot_state_publisher 
-> /tf and /tf_static.

    2. sensor_msgs/Jointstate contains effort (force, torque), position, velocity

    3. creates a bar, which is useful for debugging. TODO bar?

      • In URDF, you set it to ~use_gui = True.
      • In rosrun command line, you go _use_gui:=true

    4. default value publishes default value so your /tf is in valid state even though other parts of your system is not working

    5. Example, it looks like:

      header: 
seq: 808
stamp: 
  secs: 1572732076
  nsecs: 113363027
frame_id: ''
  name: [linear, rotational]
position: [-5.204, -0.7898988]      #your joints. 
velocity: []
effort: []

    6. in Noetic, install ros-noetic-joint-state-publisher-gui. The old joint-state-publisher still exists but not working properly in Noetic

  3. robot_state_publisher: apply forward kinematics. should run RSP, to read URDF.

    • **You gotta have: ** a launch file for
      1. launching urdf in rviz,
      2. robot_state_publisher
      3. joint_state_publisher.

  4. launch rviz, and optionally .rviz file

    1. rviz should have proper fixed frames.
    2. add "RobotModel" view to see the links on your robot
    3. add "tf" to see configuration

  5. How to get a point in rviz:

    1. add "publish tool"

    2. rostopic echo /clicked_point

Markers

  1. create markers that will automatically erase its previous position:

    visualization_msgs::Marker make_circular_marker(double x, double y, double circular_obstacle_radius, std::string frame_id,
                                                unsigned int landmark_id){
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time();
    marker.ns = "Obstacles";
    marker.id = landmark_id;
    marker.type = visualization_msgs::Marker::CYLINDER;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position.x = x;
    marker.pose.position.y = y;
    marker.pose.position.z = 0.5;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;
    marker.scale.x = circular_obstacle_radius*2;
    marker.scale.y = circular_obstacle_radius*2;
    marker.scale.z = 1.0;
    marker.color.a = 1.0; // Don't forget to set the alpha!
    marker.color.r = 0.0;
    marker.color.g = 0.0;
    marker.color.b = 1.0;
    return marker;
}

  2. Draw Path using nav_msgs/path

    void RealWorld::draw_path(){
    //TODO
    geometry_msgs::PoseStamped pose;
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = map_frame_id;
    pose.pose.position.x = x;
    pose.pose.position.y = y;
    pose.pose.orientation = quat;
    path.header.stamp = ros::Time::now();
    path.header.frame_id = map_frame_id;       //TODO: right?
    path.poses.push_back(pose);
    path_pub.publish(path);
}

  3. marker: use line list to draw lines: link

  4. Shortcut: 2D goal, 3D goal, hit g

Rviz Plugins

  1. rviz plugin needs this for gdb 
    <node pkg="rviz" type="rviz" name="rviz"
  args="-d $(find annotate_rosbags)/rviz/annotate_rosbags.rviz" output="screen" launch-prefix="xterm -e gdb --args">
</node>

Tips and Errors for Rviz

  1. URDF-> /tf?
    1. add each link slowly. you can view it in
    roslaunch urdf_tutorial display.launch model:=<path/to/urdf> (if the first link is called "base_link")

========================================================================

TF

========================================================================

Concepts

  1. transforms are assigned to each node in the transformation tree.

    • Each transform has one parent node, but unlimited number of children.
    • You can choose frame (coordinates you use to represent the system) arbitraily, but the right frame will reduce math required.
    • you need a base link, and each link is a tree structure: child and parent

  2. transform must have:

    frame_id: "body" (unique frame id)
time stamp
(see RVIZ section for example)
    • timestamp attached to the marker message is ros::Time(), which is time Zero (0), so the marker always gets displayed.
    • If you use ros::Time::now() or any other non-zero value, rviz will only display the marker if that time is close enough to the current time,
      • "close enough" depends on TF.

  3. /tf topic

    1. any node can publish and retrieve info on /tf.
    2. broadcaster: broadcasts from multiple node, and their frame_id is based on their names;
    3. each message creates an edge??
    4. Do not broadcast something that conflicts
    5. Listerner:
      1. maintain 10s history of frames. you can query the transformation bw frame A at Xs and frame b at Ys.

  4. Conventions

    1. on camera, z is forward, x is right, y is down
    2. On car, x is forward, y is left, z is up
    3. preference order of orientation: quaternion > rotation matrix SO(3) > fixed axis euler angle, XYZ > relative axis euler angle rotation: ZYX

  5. origin: center of the body. child is translted by xyz, relative to the joint to parent link.

    • roll, pitch, yall are rotated about fixed x, y, z

  6. joints: its origin and position are relative to the LAST joint

    • revolute: you have joint limits.
    • continuous: no joint limits revolute joint
      • joint_state_publisher only publishes -3.14 to 3.14 for continuous joints!
    • fixed: transformation does not change b/w two links
    • floating: all 6 dof can move
    • planar: 2dof traslational, on a plane
    • a universal joint can be composed of two revolute joints, with dummy links (you still need geometric tags, but set the size to 0).

  7. example

    1. link no rpw, the center of the box is just elevated by 0.3m. Remember the box by default is "half burried" in all dimensions. 
      <link name="right_leg">
  <visual>
    <geometry>
      <box size="0.6 0.1 0.2"/>
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0.3"/>
  </visual>
</link>
    2. #additional information is tag. by default, it's x axis; 
      #another additional thing is <limit lower="-10" upper="10" effort="100" velocity="100" /> for prismatic, revolute joints.
<joint name="rotational" type="continuous">
  <origin xyz="1.5 0 0.5" rpy="0 0 0" />
  <parent link="body" />
  <child link="arm" />
  <axis xyz="0 0 1"/>
</joint>

Basic Operations

  1. static TF: (x y z qx qy qz frame_id child_frame_id period_in_ms) 
    <launch>
  <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 base_footprint odom 100">
</launch>
    • If you have a constant TF, do it here to save bandwidth. Same mechanism as latched topic?

Code

  1. construct a tf
geometry_msgs::TransformStamped RealWorld::construct_tf(const rigid2d::Twist2D& pose_twist, const std::string& frame_id, const std::string& child_frame_id )
{
    geometry_msgs::TransformStamped trans;
    tf::StampedTransform transform;
    current_time = ros::Time::now();
    trans.header.stamp = current_time;
    trans.header.frame_id = frame_id;
    trans.child_frame_id = child_frame_id;

    trans.transform.translation.x = pose_twist.x;
    trans.transform.translation.y = pose_twist.y;
    trans.transform.translation.z = 0.0;
    geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(pose_twist.theta);
    trans.transform.rotation = quat;
    return trans;
}
  1. TransformBroadcaster 
    tf::TransformBroadcaster world_actual_robot_broadcaster;
world_actual_robot_broadcaster.sendTransform(world_actual_robot_trans);

Quat, Euler Conversions

  1. quaternion to euler:

    tf::Quaternion q(
        odom_msg.pose.pose.orientation.x,
        odom_msg.pose.pose.orientation.y,
        odom_msg.pose.pose.orientation.z,
        odom_msg.pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
auto T_sb = Transform2D(Vector2D(x_sb, y_sb), yaw);

  2. geometry_msgs::Quaternion totf::Quaternion

    #include <tf/transform_datatypes.h>
quaternionMsgToTF(quat_msg , quat_tf);

  3. tf::Quaternion to geometry_msgs::Quaternion

    #include <tf/transform_datatypes.h>
quaternionTFToMsg(quat_tf , quat_msg);

  4. quaternion conversions bw geometry_msgs and tf2

    #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion quat_tf;
geometry_msgs::Quaternion quat_msg = ...;

tf2::convert(quat_msg , quat_tf);
// or
tf2::fromMsg(quat_msg, quat_tf);
// or for the other conversion direction
quat_msg = tf2::toMsg(quat_tf);

Tips and Errors for TF

  1. if you see a white blob, you may need base_link, see here

========================================================================

Gazebo

========================================================================

Basic Gazebo Operations

  1. Running gazebo

    1. always keep rosrun gazebo_ros gazebo running,convenient
      • source devel/setup.bash
    2. spawn model
    rosrun gazebo_ros spawn_model -sdf myrobot.sdf       - run your sdf model
rosrun gazebo_ros spawn_model -file $(pwd)/rjje_arm.xacro.urdf -urdf -y 1 -model MODEL_NAME
    1. when you kill the spawn node, your model should be removed too.

  2. real time factor: simulation time vs real time

  3. Create package, see here

  4. visualizing your sdf file. link

    • try putting your .config and .world in a folder in ~/.gazebo/models/my_bot_model

  5. make launch file for your world:

    1. make sure it started in pause!

      <launch>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find ddrive)/world/ddrive.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="recording" value="false"/>
<arg name="debug" value="false"/>
      • If you pause gazebo, you pause ROS timer too

    2. Spawn your model Tricky: when you save a world model, if your robot is already in the world model, it will become part of your model!!

      <node name="mybot_spawn" pkg="gazebo_ros" type="spawn_model" output="screen"
  args="-urdf -param robot_description -model diff_drive2 -x $(arg x) -y $(arg y) -z $(arg z)" />

  6. Topics:

    • gazebo/link_states: list all link states
    • gazebo/model_states: the base of each model.

  7. Making a robot following a twist: build node that listens to cmd_vel, and outputs: services

    • /gazebo/apply_joint_effort
    • /gazebo/clear_joint_effort
    • it's generic but very inefficient because of the service calls, and interface an actual robot has

sdf

  1. Tags

    1. inertial and visual properties for links to show in Gazebo! 
      <inertia>
    2. You need an empty world link so Gazebo can load it 
      <link name="world"/>
      • To fix the model to the world
    3. URDF material tag doesnt get translated, so you need to include this separately.
      1. YOU HAVE TO SPECIFY COLORS THE URDF's Color is not being read by Gazebo
    4. mu1, mu2 show different directions, x and y.
    5. optional: <gazebo> tag for every link:
      • stl files to dae
      • sensor plugins?
    6. element for every joint
      • damping
      • actuator control plugins
    7. gazebo tags: should be placed in .gazebo file,
      • TODO include this in URDF??

  2. URDF->SDF, automatic xacro -> URDF -> SDF -> Spawn

    • Convert xacro to urdf: xacro robot.urdf.xacro > robot.urdf
    • gz sdf -p myrobot.urdf > myrobot.sdf
    • in SDF, your fixed joint connected links will be merged into one. use preserveFixedJoint>true</preserveFixedJoint>
    • gazebo tags in urdf are like decorating.?? <gazebo reference = "name"> to associate sdf specific tags with joints, links, or globally in the URDF file?
    • check: gz sdf -p URDF see warnings
    • rosrun gazebo_ros gazebo

  3. Good practice

    1. place xacro parameters in .yaml file, and include this in xacro
    2. write launch file for starting gazebo, with simulation PAUSED!!
    3. Gazebo DOESN't like zero for sizes, etc.

Tips and Errors for Gazebo

  1. 当你要publish 或subscribe 一个topic 的时候，error: list object doesn't have properties aaa, try using list indexing.

    • gazebo/model_states 是个list， 所以你要用list indexing： 
      name: [ground_plane, asphalt_plane, jersey_barrier, jersey_barrier_0, cardboard_box, cardboard_box_0, cardboard_box_1, ddrive]

  2. Debugging: if shit happens, try:

    ps aux | grep gazebo   see all gazebo processes
gazebo --verbose
killall gzserver
# or restart your computer
rosnode kill - like rosnode list, but you can kill nodes here!
ps aux | grep ros     - see all ros processes

========================================================================

Moveit!

========================================================================

  1. Concepts

    1. groups: groups of links, joints that will be planned for.
      • tip link: last link in the chain
      • base link: parent link of the first link of the chain
      • end effector: connected to parent links ONLY thru fixed joints.
        • Finally the ball will be placed on the parent link of the eef
    2. virtual joints: like the planar joint a robot car is attached to.
    3. Passive joints: like a caster
    4. self collision matrix: which links do not need to be checked for collision
    5. robot poses: home position of the robot arm.

  2. Moveit

    1. Flowchart 
      joint_states -> robot_state -> move_group -> action_server (and rviz here)
    2. Inverse Kinematics:
      • IK Fast: order of magnitude faster, ability to explore nullspace. Can check out baxter tutorial

  3. Some verified moveit topics

    /execute_trajectory/goal (empty)
/move_group/plan_execution/parameter_updates (Empty)
/move_group/trajectory_execution/parameter_updates (empty)

  4. moveit programming:

    1. get current pose 
      // get forward kinematics
robot_state::RobotStatePtr kinematic_state (move_group.getCurrentState());

const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("end_effector");

/* Print end-effector pose. Remember that this is in the model frame */
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());     //theta = 0.61, 2
    2. IK issue: KDL is not good for DOF < 6, IK_fast is not so much better either. try write your own". See this post
    3. To implement a motion controller, you need:
      • make a "controller.yaml", and put it in trajectory_execution.launch.xml. See here
      • make a JointTrajectoryAction server. See Omnid.

  5. Custom IK for moveit! post

    1. Motivation: Moveit's IKfast is only good for serial robots.

  6. Adding scene object to ROS

    roscd baxter_moveit_config
mkdir baxter_scenes
gedit baxter_scenes/baxter_pillar.scene
     #copy this 
     pillar
     * pillar
     1
     box
     0.2 0.2 1
     0.6 0.15 0
     0 0 0 1
     0 0 0 0
     .

Rico's wiki

Home | Copyright © 2021 ricojia

⚠️ **GitHub.com Fallback** ⚠️