Ros Rviz Add URDF file display Robot Model _ robot

The following code launches the Rviz and loads the URDF file format robot Model:
Although some do not know, but is able to start and load the robot correctly.

<launch>

  <arg name= "model" default= "/HOME/XXX/3D_MODEL/SRC/ROBOT1_DESCRIPTION/URDF/ROBOT1.URDF" >
  <arg name= "GUI" default= "true"/>
  <arg name= "Rvizconfig" default= "$ (find urdf_tutorial)/rviz/ Urdf.rviz "/>

  <param name=" Robot_description "command=" $ (Find Xacro)/xacro.py $ (arg model) "/>
  < param name= "Use_gui" value= "$ (arg gui)"/> <node name= "joint_state_publisher"

  pkg= "Joint_state_publisher" Type= "Joint_state_publisher"/> <node name= "Rviz" pkg= "Rviz" type= "Rviz" args= "- 

  D $ (arg rvizconfig)" Required= "true"/>

</launch>

as follows, the URDF file contents for the robot:

<?xml version= "1.0"?> <robot name= "Origins" > <link name= "Base_link" > <visual> <geometry> <box size= "0.2.3.1"/> </geometry> & Lt;origin rpy= "0 0 0" xyz= "0 0 0.05"/> <material name= "white" > <color rgba= "1 1 1
                    1 "/> </material> </visual> <collision> <geometry> <box size= "0.2.3 0.1"/> </geometry> </collision> <inertia l> <mass value= "/> <inertia ixx=" 1.0 "ixy=" 0.0 "ixz=" 0.0 "iyy=" 1.0 "iyz=" 0.0
                "1.0"/> </inertial> </link> <link name= "Wheel_1" > <visual> 
            <geometry> <cylinder length= "0.05" radius= "0.05"/> </geometry> <originrpy= "0 1.5 0" xyz= "0 0 0"/> <material name= "Black" > <color rgba= "0 0 0 1"/>
                    </material> </visual> <collision> <geometry> <cylinder length= "0.05" radius= "0.05"/> </geometry> </collision> <ine Rtial> <mass value= "ten"/> <inertia ixx= "1.0" ixy= "0.0" ixz= "0.0" iyy= "1.0" iyz= "0.0" I
                zz= "1.0"/> </inertial> </link> <link name= "wheel_2" > <visual> <geometry> <cylinder length= "0.05" radius= "0.05"/> &LT;/GEOMETRY&G
            T <origin rpy= "0 1.5 0" xyz= "0 0 0"/> <material name= "Black"/> </visual> <c ollision> <geometry> <cylinder length= "0.05" radius= "0.05"/> &L T;/geometry> </collision> <inertial> <mass value= "Ten"/> <inertia IX X= "1.0" ixy= "0.0" ixz= "0.0" iyy= "1.0" iyz= "0.0" izz= "1.0"/> </inertial> </link> <link Name= "Wheel_3" > <visual> <geometry> <cylinder length= "0.05" radius= "0.05"/> </geometry> <origin rpy= "0 1.5 0" xyz= "0 0 0"/>
                    ; material name= "Black"/> </visual> <collision> <geometry> <cylinder length= "0.05" radius= "0.05"/> </geometry> </collision> <in Ertial> <mass value= "ten"/> <inertia ixx= "1.0" ixy= "0.0" ixz= "0.0" iyy= "1.0" iyz= "0.0"
                izz= "1.0"/> </inertial> </link> <link name= "Wheel_4" > <visual> <geometry> <cylinder length= "0.05" radius= "0.05"/> </geometry> <origin rpy= "0 1.5 0" xyz= " 0 0 0 "/> <material name=" Black "/> </visual> <collision> <g eometry> <cylinder length= "0.05" radius= "0.05"/> </geometry> </c ollision> <inertial> <mass value= "ten"/> <inertia ixx= "1.0" ixy= "0.0" ixz = "0.0" iyy= "1.0" iyz= "0.0" izz= "1.0"/> </inertial> </link> <joint name= "Base_to_wheel1 "type=" continuous "> <parent link=" base_link "/> <child link=" Wheel_1 "/> <origin Xyz= "0.1 0.1 0"/> <axis xyz= "1 0 0"/> </joint> <joint name= "Base_to_wheel2" Typ e= "continuous" > <parent link= "base_link"/> <child link= "wheel_2"/> <origin "xyz=" -0.1 0.1 0 "/> <axis xyz=" 1 0 0 "/> </joint> <joint name=" Base_to_wheel3 "type= "Continuous" > <parent link= "base_link"/> <child link= "wheel_3"/> <origin "xyz=" 0. 1-0.1 0 "/> <axis xyz=" 1 0 0 "/> </joint> <joint name=" Base_to_wheel4 "Type=" Co Ntinuous "> <parent link=" base_link/> <child link= "wheel_4"/> <origin xyz= "-0.1 
-0.1 0 "/> <axis xyz=" 1 0 0 "/> </joint> <link name=" Arm_base "> <visual> <geometry> <box size= "0.1.1.1"/> </geometry> <origin rpy= "0 0 0" xyz= "0 0 0.1"/> <material n Ame= "White" > <color rgba= "1 1 1 1"/> </material> </visual> <collision> <geometry> < Box size= "0.1 1.1"/> </geometry> </collision> <inertial> <mass value= "1"/> <inertia ixx= "1.0" ixy= "0.0" ixz= "0.0" iyy= "1.0" iyz= "0.0" izz= "1.0"/> </inertial> </link> <joint name= "base_to_arm_base" type= "continuous" 
> <parent link= "base_link"/> <child link= "arm_base"/> <axis xyz= "0 0 1"/> <origin xyz= "0 0 0"/> </joint> <link name= "arm_1" > <visual> <geometry> <box size= "0.05.0.5"/> </geometry > <origin rpy= "0 0 0" xyz= "0 0 0.25"/> <material name= "white" > <color rgba= "1 1 1 1"/> </material&
Gt </visual> <collision> <geometry> <box size= "0.05 0.5"/> </geometry> </collision > <inertial> <mass value= "1"/> <inertia ixx= "1.0" ixy= "0.0" ixz= "0.0" iyy= "1.0" iyz= "0.0" izz= "1.0"/&
Gt </inertial> </link> <joint name= "arm_1_to_arm_base" type= "Revolute" > <parent link= "arm_base" > <child link= "arm_1"/> <axis xyz= "1 0 0"/> <origin xyz= "0 0 0.15"/> <limit effort = "1000.0" lower = " -2.0" upper= "1.0" velocity= "0.5"/> &Lt;/joint> <link name= "arm_2" > <visual> <geometry> <box size= "0.05 0.05 0.5"/> </geometry&
Gt <origin rpy= "0 0 0" xyz= "0.06 0 0.15"/> <material name= "white" > <color rgba= "1 1 1 1"/> </material&gt
; </visual> <collision> <geometry> <box size= "0.05 0.5"/> </geometry> </collision > <inertial> <mass value= "1"/> <inertia ixx= "1.0" ixy= "0.0" ixz= "0.0" iyy= "1.0" iyz= "0.0" izz= "1.0"/&
Gt </inertial> </link> <joint name= "arm_2_to_arm_1" type= "Revolute" > <parent link= "arm_1"/> <  Child link= "Arm_2"/> <axis xyz= "1 0 0"/> <origin xyz= "0.0 0 0.45"/> "<limit effort =" 1000.0 "lower="-2.5 " Upper= "2.5" velocity= "0.5"/> </joint> <joint name= "Left_gripper_joint" type= "Revolute" > <axis xyz= "  0 0 1 "/> <limit effort=" 1000.0 "lower=" 0.0 "upper=" "0.548" velocity= "0.5"/> <origin rpy= "0-1.57 0" xyz= "0.06 0 0.4 "/> <parent link=" Arm_2 "> <child link= "left_gripper"/> </joint> <link name= "Left_gripper" > <visual> <origin rpy= "0 0 0" xyz= "0 0 0"/> <geometry> <mesh filename= "Package://pr2_description/meshes/gripper_v0/l_finger.dae" /> </geometry> </visual> <collision> <geometry> <box size= "0.1.1.1"/> </geometry&
Gt </collision> <inertial> <mass value= "1"/> <inertia ixx= "1.0" ixy= "0.0" ixz= "0.0" iyy= "1.0" iyz= " 0.0 "izz=" 1.0 "/> </inertial> </link> <joint name=" Left_tip_joint "type=" fixed "> <parent link=" Left_gripper "/> <child link=" Left_tip "/> </joint> <link name=" Left_tip "> <visual> < Origin rpy= "0.0 0 0" xyz= "0.09137 0.00495 0"/> <geometry> <mesh filename= "package://pr2_description/meshes/ Gripper_v0/l_finger_tip.dae "/> </geometry> </visual> <collision> <geometry> <box size= "0.1 1.1"/> </geometry> </collision> <Inertial> <mass value= "1"/> <inertia ixx= "1.0" ixy= "0.0" ixz= "0.0" iyy= "1.0" iyz= "0.0" izz= "1.0"/> </ inertial> </link> <joint name= "Right_gripper_joint" type= "Revolute" > <axis xyz= "0 0-1"/> < Limit effort= "1000.0" lower= "0.0" upper= "0.548" velocity= "0.5"/> <origin rpy= "0-1.57 0" xyz= "0.06 0 0.4"/> <p Arent link= "arm_2"/> <child link= "Right_gripper"/> </joint> <link name= "Right_gripper" > < visual> <origin rpy= " -3.1415 0 0" xyz= "0 0 0"/> <geometry> <mesh filename= "package://pr2_description/ Meshes/gripper_v0/l_finger.dae "/> </geometry> </visual> <collision> <geometry> <box Size= "0.1 1.1"/> </geometry> </collision> <inertial> <mass value= "1"/> <inertia ixx= " 1.0 "ixy=" 0.0 "ixz=" 0.0 "iyy=" 1.0 "iyz=" 0.0 "izz=" 1.0 "/> </inertial> </link> <joint name=" Right_tip_ Joint "type=" "fixed" > <parent link= "right_gripper"/> &LT;child link= "Right_tip"/> </joint> <link name= "Right_tip" > <visual> <origin rpy= "-3.1415 0 0" xyz= "0.09137 0.00495 0"/> <geometry> <mesh filename= "Package://pr2_description/meshes/gripper_v0/l_ Finger_tip.dae "/> </geometry> </visual> <collision> <geometry> <box size=" 0.1.1.1 "/ > </geometry> </collision> <inertial> <mass value= "1"/> <inertia ixx= "1.0" ixy= "0.0" ixz= "0.0" iyy= "1.0" iyz= "0.0" izz= "1.0"/> </inertial> </link> </robot>

The application that publishes the message can control the movement of the picker and the rotation of the wheel, but the car has not yet moved ...

#include <string> #include <ros/ros.h> #include <sensor_msgs/JointState.h> #include <tf/
    transform_broadcaster.h> int main (int argc, char** argv) {ros::init (argc, argv, "State_publisher");
    Ros::nodehandle N;
    Ros::P ublisher joint_pub = n.advertise<sensor_msgs::jointstate> ("Joint_states", 1);
    Tf::transformbroadcaster broadcaster;

    Ros::rate Loop_rate (30);

    Const double degree = m_pi/180;
    Double height=0; Robot state Double inc= 0.005, base_arm_inc= 0.005, arm1_armbase_inc= 0.005, arm2_arm1_inc= 0.005, gripper_inc= 0.0
    Tip_inc= 0.005;
    Double angle= 0, base_arm = 0, arm1_armbase = 0, Arm2_arm1 = 0, gripper = 0, tip = 0;
    Message declarations geometry_msgs::transformstamped Odom_trans;
    Sensor_msgs::jointstate joint_state;
    odom_trans.header.frame_id = "Odom";

    odom_trans.child_frame_id = "Base_link"; while (Ros::ok ()) {//update Joint_state joint_state.header.stamp = Ros::tiMe::now ();
        Joint_state.name.resize (11);
        Joint_state.position.resize (11);
        Joint_state.name[0] = "Base_to_arm_base";
        Joint_state.position[0] = Base_arm;
        JOINT_STATE.NAME[1] = "Arm_1_to_arm_base";
        JOINT_STATE.POSITION[1] = arm1_armbase;
        JOINT_STATE.NAME[2] = "arm_2_to_arm_1";
    JOINT_STATE.POSITION[2] = Arm2_arm1;
        JOINT_STATE.NAME[3] = "Left_gripper_joint";
    JOINT_STATE.POSITION[3] = gripper;
        JOINT_STATE.NAME[4] = "Left_tip_joint";
    JOINT_STATE.POSITION[4] = tip;
        JOINT_STATE.NAME[5] = "Right_gripper_joint";
    JOINT_STATE.POSITION[5] = gripper;
        JOINT_STATE.NAME[6] = "Right_tip_joint";
    JOINT_STATE.POSITION[6] = tip;
        JOINT_STATE.NAME[7] = "Base_to_wheel1";
    JOINT_STATE.POSITION[7] =height;
        JOINT_STATE.NAME[8] = "Base_to_wheel2";
    JOINT_STATE.POSITION[8] = height;
        JOINT_STATE.NAME[9] = "BASE_TO_WHEEL3";
    JOINT_STATE.POSITION[9] = height; JOINT_STATE.NAME[10] = "Base_to_wheel4 ";

        JOINT_STATE.POSITION[10] = height;





        height+=0.005;
        Update transform//(moving in a circle with radius) Odom_trans.header.stamp = Ros::time::now ();
        odom_trans.transform.translation.x = cos (angle);
        ODOM_TRANS.TRANSFORM.TRANSLATION.Y = sin (angle);
        odom_trans.transform.translation.z = 0.0;

        Odom_trans.transform.rotation = Tf::createquaternionmsgfromyaw (angle);
        Send the joint State and Transform Joint_pub.publish (joint_state);


    Broadcaster.sendtransform (Odom_trans);
        Create new robot state arm2_arm1 + = Arm2_arm1_inc;
          if (arm2_arm1<-1.5 | | arm2_arm1>1.5) arm2_arm1_inc *=-1;
        Arm1_armbase + = Arm1_armbase_inc;
        if (arm1_armbase>1.2 | | arm1_armbase<-1.0) arm1_armbase_inc *=-1;
        Base_arm + = Base_arm_inc;
        if (base_arm>1. | | base_arm<-1.0) base_arm_inc *=-1;
        Gripper + = Gripper_inc; if (gripper<0 | |

          gripper>1) Gripper_inc *=-1;

        Angle + = DEGREE/4;
    This is adjust as needed per iteration loop_rate.sleep ();
return 0; }