ROS/Gazebo练习2: stack light simulation

light_description/urdf/light.xacro

<?xml version="1.0"?>
<!-- light -->
<robot name="light" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Import Rviz colors -->
  <xacro:include filename="$(find robot_description)/urdf/materials.xacro" />

  <!-- Constants for robot dimensions -->
  <xacro:property name="radius_BL" value="0.02" />
  <xacro:property name="radius_G" value="0.02" /> 
  <xacro:property name="radius_Y" value="0.02" /> 
  <xacro:property name="radius_R" value="0.02" /> 
  <xacro:property name="radius_TP" value="0.02" /> 
 
  <xacro:property name="length_BL" value="0.08" />
  <xacro:property name="length_G" value="0.04" /> 
  <xacro:property name="length_Y" value="0.04" /> 
  <xacro:property name="length_R" value="0.04" /> 
  <xacro:property name="length_TP" value="0.01" /> 
 
  <!-- Used for fixing robot to Gazebo 'base_link' -->
  <link name="world"/>

  <joint name="base_link_to_world_joint" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>
  
  <!-- LINK_BASE -->
  <link name="base_link">
    <collision>
      <origin xyz="0 0 ${length_BL/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_BL}" radius="${radius_BL}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${length_BL/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_BL}" radius="${radius_BL}"/>
      </geometry>
      <material name="white"/>
    </visual>

    <inertial>
       <mass value="0.121"/>
        <origin xyz="1.08E-04 -1.03E-03 2.08E-02" rpy="0 0 0"/>
        <inertia ixx="3.20E-05" ixy="-9.45E-08" ixz="2.61E-07"
          iyy="4.14E-05" iyz="3.88E-07"
          izz="4.29E-05"/>
    </inertial>
  </link>

  <!-- JOINT_1 -->
  <joint name="joint_1" type="fixed">
    <parent link="base_link"/>
    <child link="link_g"/>
    <origin xyz="0 0 ${length_BL}" rpy="0 0 0"/>
  </joint>

  <!-- LINK_GREEN -->
  <link name="link_g">
    <collision>
      <origin xyz="0 0 ${length_G/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_G}" radius="${radius_G}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${length_G/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_G}" radius="${radius_G}"/>
      </geometry>
      <material name="green"/>
    </visual>

    <inertial>
       <mass value="0.121"/>
        <origin xyz="1.08E-04 -1.03E-03 2.08E-02" rpy="0 0 0"/>
        <inertia ixx="3.20E-05" ixy="-9.45E-08" ixz="2.61E-07"
          iyy="4.14E-05" iyz="3.88E-07"
          izz="4.29E-05"/>
    </inertial>
  </link>

  <!-- JOINT_2 -->
  <joint name="joint_2" type="fixed">
    <parent link="link_g"/>
    <child link="link_y"/>
    <origin xyz="0 0 ${length_G}" rpy="0 0 0"/>
  </joint>

  <!-- LINK_Y -->
  <link name="link_y">
    <collision>
      <origin xyz="0 0 ${length_Y/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_Y}" radius="${radius_Y}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${length_Y/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_Y}" radius="${radius_Y}"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <inertial>
       <mass value="0.121"/>
        <origin xyz="1.08E-04 -1.03E-03 2.08E-02" rpy="0 0 0"/>
        <inertia ixx="3.20E-05" ixy="-9.45E-08" ixz="2.61E-07"
          iyy="4.14E-05" iyz="3.88E-07"
          izz="4.29E-05"/>
    </inertial>
  </link>

  <!-- JOINT_3 -->
  <joint name="joint_3" type="fixed">
    <parent link="link_y"/>
    <child link="link_r"/>
    <origin xyz="0 0 ${length_Y}" rpy="0 0 0"/>
  </joint>

  <!-- LINK_R -->
  <link name="link_r">
    <collision>
      <origin xyz="0 0 ${length_R/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_R}" radius="${radius_R}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${length_R/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_R}" radius="${radius_R}"/>
      </geometry>
      <material name="red"/>
    </visual>

    <inertial>
       <mass value="0.121"/>
        <origin xyz="1.08E-04 -1.03E-03 2.08E-02" rpy="0 0 0"/>
        <inertia ixx="3.20E-05" ixy="-9.45E-08" ixz="2.61E-07"
          iyy="4.14E-05" iyz="3.88E-07"
          izz="4.29E-05"/>
    </inertial>
  </link>

  <!-- JOINT_4 -->
  <joint name="joint_4" type="fixed">
    <parent link="link_r"/>
    <child link="link_top"/>
    <origin xyz="0 0 ${length_R}" rpy="0 0 0"/>
  </joint>

  <!-- LINK_TOP -->
  <link name="link_top">
    <collision>
      <origin xyz="0 0 ${length_TP/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_TP}" radius="${radius_TP}"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 ${length_TP/2}" rpy="0 0 0"/>
      <geometry>
	      <cylinder length="${length_TP}" radius="${radius_TP}"/>
      </geometry>
      <material name="white"/>
    </visual>

    <inertial>
       <mass value="0.121"/>
        <origin xyz="1.08E-04 -1.03E-03 2.08E-02" rpy="0 0 0"/>
        <inertia ixx="3.20E-05" ixy="-9.45E-08" ixz="2.61E-07"
          iyy="4.14E-05" iyz="3.88E-07"
          izz="4.29E-05"/>
    </inertial>
  </link>

  <!-- For gazebo simulation -->
    <!-- color for gazebo simulation-->
    <gazebo reference="base_link">
      <material>Gazebo/White</material>
    </gazebo>
    <gazebo reference="link_g">
      <material>Gazebo/Grey</material>
    </gazebo>
    <gazebo reference="link_y">
      <material>Gazebo/Grey</material>
    </gazebo>
    <gazebo reference="link_r">
      <material>Gazebo/Grey</material>
    </gazebo>
    <gazebo reference="link_top">
      <material>Gazebo/White</material>
    </gazebo> 

</robot>

light_command/stack_light_msgs

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(stack_light_msgs)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  message_generation
  std_msgs
)

add_service_files(
  FILES
  light_srv.srv
)

generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
 CATKIN_DEPENDS
 message_runtime
)

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>stack_light_msgs</name>
  <version>0.0.0</version>
  <description>The stack_light_msgs package</description>

  <maintainer email="wuzepei@todo.todo">wuzepei</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>message_generation</build_depend>
  <build_depend>std_msgs</build_depend>

  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>

  <export></export>
</package>

/srv/light_srv.srv

uint8 red
uint8 yellow
uint8 green
---
uint8 red
uint8 yellow
uint8 green

light_command/stack_light

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(stack_light)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  stack_light_msgs
)

catkin_package(
  CATKIN_DEPENDS 
  message_runtime
  stack_light_msgs 
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(stacklight
  src/LightNode.cpp
)

target_link_libraries(stacklight ${catkin_LIBRARIES})

add_dependencies(stacklight ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>stack_light</name>
  <version>0.0.0</version>
  <description>The stack_light package</description>

  <maintainer email="wuzepei@todo.todo">wuzepei</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>stack_light_msgs</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>stack_light_msgs</exec_depend>

  <export></export>
</package>

/src/LightNode.cpp

/**
 * stack_light Server
 */

#include <string>
#include "ros/ros.h"
#include <stack_light_msgs/light_srv.h>


// @param [in] &req.red        red light on
// @param [in] &req.yellow     yellow light on
// @param [in] &req.green      green light on

// @param [out]  &res.result   all lights result 
// @param [out]  &res.red      red light state
// @param [out]  &res.yellow   yellow light state
// @param [out]  &res.green    green light state


void lightOn(std::string color)
{
    ROS_INFO("%s light is ON",color.c_str());
    // TODO
}

void lightOff(std::string color)
{
    ROS_INFO("%s light is Off",color.c_str());
    // TODO
}

// service回调函数，输入参数req，输出参数res
bool colorJudgment(stack_light_msgs::light_srv::Request  &req,
         stack_light_msgs::light_srv::Response &res)
{
    // 将输入参数中的请求数据相加，结果放到应答变量中
    if(req.red == 0) 
    {
        lightOff("red");
        res.red = 0;
    }
    else
    {
        lightOn("red");
        res.red = 1;
    }

    if(req.yellow == 0)
    {
        lightOff("yellow");
        res.yellow = 0;
    }
    else
    {
        lightOn("yellow");
        res.yellow = 1;
    }

    if(req.green == 0)
    {
        lightOff("green");
        res.green = 0;
    }
    else
    {
        lightOn("green");
        res.green = 1;
    }
    
    return true;
}

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "lightNode");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个名为add_two_ints的server，注册回调函数add()
    ros::ServiceServer service = n.advertiseService("light_srv", colorJudgment);

    // 循环等待回调函数
    ROS_INFO("Ready to add light_srv.");
    ros::spin();

    return 0;
}