diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5f13db926b2b0b10708c48a4e0ba9fd1f5c7bbd3..8a3b0c75d6c536eea2c16132b6d4c21903e7fa69 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
-project(color_plugin)
+project(iri_gazebo_set_material_plugin)
# set(CMAKE_BUILD_TYPE "RelWithDebInfo")
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
@@ -23,7 +23,7 @@ include_directories(
link_directories( ${GAZEBO_LIBRARY_DIRS})
-add_library(${PROJECT_NAME} src/color_plugin.cc)
+add_library(${PROJECT_NAME} src/iri_gazebo_set_material_plugin.cc)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
#############
diff --git a/README.md b/README.md
index 6bb680626c1405e5ab5cc17d5ede926b907c2989..2544f252f839f375364e59eab1191733ad5c76b9 100644
--- a/README.md
+++ b/README.md
@@ -1,61 +1,50 @@
# Description
-This package is a Gazebo plugin to control the state of a single sempahores in the model car simulator. This package is part of the model car simulator used for the [ADC competition](http://autonomousdrivingchallenge.com/ "ADC competition").
+This package is a Gazebo plugin to change a model reference material.
+For example, it is used to change the state of a simulated sempahore light from [iri_sign_description](https://gitlab.iri.upc.edu/mobile_robotics/adc/adc_2021/iri_sign_description)
-This package in intended to be used with ROS Kinetic under Linux Ubuntu 16.04, although it may work with other ROS versions and Linux variations.
+This package has been tested ROS Melodic (Gazebo 9) under Linux Ubuntu 18.04, although it may work with other ROS-Gazebo versions and Linux variations.
-# Dependencies
-
-This node has the following dependencies:
-
- * gazebo ros
-
- * gazebo plugins
-
-All these dependencies can be installed with:
+# Installation
+Move to the active workspace:
```
-sudo apt-get install ros-kinetic-gazebo-ros ros-kinetic-gazebo-plugins
+roscd && cd ../src
```
-# How to use it
-
-## Install
-
-This package can be cloned to the active workspace with the following command:
+Clone the repository:
+```
+git clone <url>
+```
+Install all ROS dependencies with the following commands:
```
roscd
-cd ../src
-git clone https://gitlab.iri.upc.edu/seat_adc/simulator/color_plugin.git
+cd ..
+rosdep install -i -r --from-paths src
```
-But it is recommended to follow the [instructions on how to install the model simulator](https://gitlab.iri.upc.edu/seat_adc/simulator/model_car_gazebo).
-
-## running
-
-Each semaphore will provide a service call to change its state. The namespace of the service call depend on the name given to the semaphore. The default model cal simulator provides the following services:
-
- * /semaphore_intersection1_color_plugin/trigger
+Compile all the ROS packages with the following commands:
+```
+catkin_make
+```
- * /semaphore_intersection2_color_plugin/trigger
-
- * /semaphore_intersection3_color_plugin/trigger
- * /semaphore_parking1_color_plugin/trigger
+# How to use it
- * /semaphore_parking2_color_plugin/trigger
+See example files provided:
- * /semaphore_roundabout1_color_plugin/trigger
+* urdf/sample_box_macro.xacro: macro of a box model using the plugin.
+* urdf/sample_box_example.xacro: example instantiation of previous macro
+* launch/spawn.launch: launch example to spawn a box in gazebo.
- * /semaphore_roundabout2_color_plugin/trigger
+## Launch
- * /semaphore_start1_color_plugin/trigger
+`roslaunch gazebo_ros empty_world.launch`
- * /semaphore_start2_color_plugin/trigger
+`roslaunch iri_gazebo_set_material_plugin spawn.launch`
-To change the state of any of these semaphores, call the corresponding service:
+`rostopic pub /sample_box_set_material_plugin/set_material std_msgs/String "data: ''"`
-```
-rosservice call /semaphore_start1_color_plugin/trigger
+`rostopic pub /sample_box_set_material_plugin/set_material std_msgs/String "data: 'Gazebo/Blue'"`
```
diff --git a/include/color_plugin.h b/include/iri_gazebo_set_material_plugin.h
similarity index 89%
rename from include/color_plugin.h
rename to include/iri_gazebo_set_material_plugin.h
index b24e85ad72a9eb892e58d6732c12a7f74589f68b..4a9f30212fc4873de4eaf721d68aa7bc29092ba4 100644
--- a/include/color_plugin.h
+++ b/include/iri_gazebo_set_material_plugin.h
@@ -15,8 +15,8 @@
*
*/
-#ifndef _GAZEBO_LIGHT_PLUGIN_HH_
-#define _GAZEBO_LIGHT_PLUGIN_HH_
+#ifndef _IRI_GAZEBO_SET_MATERIAL_PLUGIN_HH_
+#define _IRI_GAZEBO_SET_MATERIAL_PLUGIN_HH_
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
@@ -31,12 +31,12 @@
//namespace gazebo
//{
- class ColorPlugin : public gazebo::VisualPlugin
+ class IriGazeboSetMaterialPlugin : public gazebo::VisualPlugin
{
public:
- ColorPlugin();
- virtual ~ColorPlugin();
+ IriGazeboSetMaterialPlugin();
+ virtual ~IriGazeboSetMaterialPlugin();
void Load(gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sdf);
void OnUpdate();
private:
diff --git a/launch/spawn.launch b/launch/spawn.launch
new file mode 100644
index 0000000000000000000000000000000000000000..135463eb8229b33be42785263faf40e07f2298f7
--- /dev/null
+++ b/launch/spawn.launch
@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+ <arg name="name" default="sample_box"/>
+ <arg name="model" default="sample_box_example"/>
+ <arg name="x" default="2.0"/>
+ <arg name="y" default="0.0"/>
+ <arg name="yaw" default="0.0"/>
+ <arg name="parent" default="map"/>
+
+ <param name="$(arg name)_description"
+ command="$(find xacro)/xacro '$(find iri_gazebo_set_material_plugin)/urdf/$(arg model).xacro'
+ name:=$(arg name)">
+ </param>
+
+ <node name="$(arg name)_state_publisher"
+ pkg ="robot_state_publisher"
+ type="robot_state_publisher">
+ <!--<param name="tf_prefix" value="/$(arg name)" type="str" />-->
+ <remap from="robot_description" to="$(arg name)_description" />
+ <remap from="/joint_states" to="/$(arg name)/joint_states" />
+ </node>
+
+ <node name="spawn_urdf_$(arg name)"
+ pkg ="gazebo_ros"
+ type="spawn_model"
+ args="-param /$(arg name)_description -urdf -model $(arg name) -x $(arg x) -y $(arg y) -z 0 -Y $(arg yaw)">
+ </node>
+
+ <node name="static_tf_$(arg name)_base_link_to_$(arg parent)" pkg="tf" type="static_transform_publisher"
+ args="$(arg x) $(arg y) 0 $(arg yaw) 0 0 $(arg parent) $(arg name)_base_link 100">
+ </node>
+
+</launch>
diff --git a/package.xml b/package.xml
index 85ec3c8a11268ed4d759527314dc322ed1359c39..270f269dbe7f496dff796b7e329bbc8e9c4b43b0 100644
--- a/package.xml
+++ b/package.xml
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package>
- <name>color_plugin</name>
+ <name>iri_gazebo_set_material_plugin</name>
<version>1.0.0</version>
- <description>The color_plugin package</description>
+ <description>The iri_gazebo_set_material_plugin package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
diff --git a/src/color_plugin.cc b/src/iri_gazebo_set_material_plugin.cc
similarity index 73%
rename from src/color_plugin.cc
rename to src/iri_gazebo_set_material_plugin.cc
index 71c6a6814afdb125076265225381199d6176b92d..554a8ec5a7eb9bb79628360e93bb761f09c727ff 100644
--- a/src/color_plugin.cc
+++ b/src/iri_gazebo_set_material_plugin.cc
@@ -14,13 +14,13 @@
* limitations under the License.
*
*/
-#include "color_plugin.h"
+#include "iri_gazebo_set_material_plugin.h"
//using namespace gazebo;
-GZ_REGISTER_VISUAL_PLUGIN(ColorPlugin)
+GZ_REGISTER_VISUAL_PLUGIN(IriGazeboSetMaterialPlugin)
-ColorPlugin::ColorPlugin()
+IriGazeboSetMaterialPlugin::IriGazeboSetMaterialPlugin()
{
this->nh = NULL;
this->material_name="Gazebo/Red";
@@ -34,12 +34,12 @@ ColorPlugin::ColorPlugin()
}
-ColorPlugin::~ColorPlugin()
+IriGazeboSetMaterialPlugin::~IriGazeboSetMaterialPlugin()
{
delete this->nh;
}
-void ColorPlugin::Load(gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sdf)
+void IriGazeboSetMaterialPlugin::Load(gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sdf)
{
this->model = _parent;
@@ -60,11 +60,11 @@ void ColorPlugin::Load(gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sd
this->new_material=true;
}
- this->pluginThread = boost::thread(boost::bind(&ColorPlugin::threadFunction, this));
- this->updateConnection = gazebo::event::Events::ConnectPreRender(boost::bind(&ColorPlugin::OnUpdate, this));
+ this->pluginThread = boost::thread(boost::bind(&IriGazeboSetMaterialPlugin::threadFunction, this));
+ this->updateConnection = gazebo::event::Events::ConnectPreRender(boost::bind(&IriGazeboSetMaterialPlugin::OnUpdate, this));
}
-void ColorPlugin::threadFunction()
+void IriGazeboSetMaterialPlugin::threadFunction()
{
if(!ros::isInitialized())
{
@@ -78,13 +78,13 @@ void ColorPlugin::threadFunction()
std::string material_topic = "/" + this->plugin_name + "/" + "set_material";
//std::string color_topic = "/" + this->plugin_name + "/" + "set_color";
//std::string srv_topic = "/" + this->plugin_name + "/" + "trigger";
- this->material_subscriber = this->nh->subscribe<std_msgs::String>(material_topic, 1, boost::bind(&ColorPlugin::materialCallback, this, _1));
- //this->color_subscriber = this->nh->subscribe<std_msgs::ColorRGBA>(color_topic, 1, boost::bind(&ColorPlugin::colorCallback, this, _1));
- //this->trigger_server = this->nh->advertiseService(srv_topic, &ColorPlugin::triggerCallback, this);
+ this->material_subscriber = this->nh->subscribe<std_msgs::String>(material_topic, 1, boost::bind(&IriGazeboSetMaterialPlugin::materialCallback, this, _1));
+ //this->color_subscriber = this->nh->subscribe<std_msgs::ColorRGBA>(color_topic, 1, boost::bind(&IriGazeboSetMaterialPlugin::colorCallback, this, _1));
+ //this->trigger_server = this->nh->advertiseService(srv_topic, &IriGazeboSetMaterialPlugin::triggerCallback, this);
}
}
-void ColorPlugin::materialCallback(const std_msgs::String::ConstPtr& _msg)
+void IriGazeboSetMaterialPlugin::materialCallback(const std_msgs::String::ConstPtr& _msg)
{
std_msgs::String name_msg = *_msg;
@@ -92,7 +92,7 @@ void ColorPlugin::materialCallback(const std_msgs::String::ConstPtr& _msg)
{
if(this->material_name==this->material1_name)
this->material_name=this->material2_name;
- else if(this->material_name==this->material2_name)
+ else //if(this->material_name==this->material2_name)
this->material_name=this->material1_name;
}
else
@@ -101,13 +101,13 @@ void ColorPlugin::materialCallback(const std_msgs::String::ConstPtr& _msg)
this->new_material=true;
}
-// void ColorPlugin::colorCallback(const std_msgs::ColorRGBA::ConstPtr& _msg)
+// void IriGazeboSetMaterialPlugin::colorCallback(const std_msgs::ColorRGBA::ConstPtr& _msg)
// {
// this->color.Set(_msg->r, _msg->g, _msg->b);
// this->new_color=true;
// }
-// bool ColorPlugin::triggerCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
+// bool IriGazeboSetMaterialPlugin::triggerCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
// {
// if(!this->trigger_state)
// this->material_name=this->material2_name;
@@ -118,7 +118,7 @@ void ColorPlugin::materialCallback(const std_msgs::String::ConstPtr& _msg)
// return true;
// }
-void ColorPlugin::OnUpdate()
+void IriGazeboSetMaterialPlugin::OnUpdate()
{
if(this->new_material)
{
diff --git a/urdf/box_macro.xacro b/urdf/box_macro.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..fed9a24510f54071372048338e039b2d0026f9b0
--- /dev/null
+++ b/urdf/box_macro.xacro
@@ -0,0 +1,123 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+
+
+ <xacro:property name="PI" value="3.1415926535897931" />
+ <xacro:property name="box_width" value="0.5" />
+ <xacro:property name="box_depth" value="0.5" />
+ <xacro:property name="box_height" value="0.5" />
+
+ <xacro:macro name="sample_box"
+ params="name:=semaphore
+ tag_dae:=alvar0_negative
+ box_gazebo_material:=Gazebo/FlatBlack
+ light_gazebo_material:=Gazebo/Green
+ light_gazebo_material2:=Gazebo/Red">
+
+ <link name="${name}_base_link">
+ <visual>
+ <geometry>
+ <!--<box size="${box_depth} ${box_width} ${box_height}"/>-->
+ <mesh filename="package://iri_sign_description/meshes/semaphore_case.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <material name="${box_material}"/>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://iri_sign_description/meshes/semaphore_case.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="${name}_base_link">
+ <material>${box_gazebo_material}</material>
+ </gazebo>
+
+ <link name="${name}_light_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://iri_sign_description/meshes/semaphore_light.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ </visual>
+ <visual>
+ <origin xyz="-0.042 0 0" rpy="0 0 3.14159" />
+ <geometry>
+ <mesh filename="package://iri_sign_description/meshes/semaphore_light.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ </visual>
+ </link>
+
+ <gazebo reference="${name}_light_link">
+ <!--<material>${light_gazebo_material}</material>-->
+
+ <visual>
+ <plugin name="${name}_set_material_plugin" filename="libiri_gazebo_set_material_plugin.so" >
+ <material>${light_gazebo_material}</material>
+ <material2>${light_gazebo_material2}</material2>
+ </plugin>
+ </visual>
+
+ </gazebo>
+
+ <joint name="joint_${name}_base_link_to_${name}_light_link" type="fixed">
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <parent link="${name}_base_link"/>
+ <child link="${name}_light_link" />
+ </joint>
+
+ <link name="${name}_tag_link">
+ <inertial>
+ <mass value="0.001"/>
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+ <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
+ </inertial>
+ </link>
+
+ <joint name="joint_${name}_base_link_to_${name}_tag_link" type="fixed">
+ <origin xyz="${delta} 0 ${tag_size/2.0+tag_z_offset}" rpy="0 0 0" />
+ <parent link="${name}_base_link"/>
+ <child link="${name}_tag_link" />
+ </joint>
+
+ <gazebo reference="${name}_tag_link">
+ <!--<material>${box_gazebo_material}</material>-->
+ </gazebo>
+
+ <link name="${name}_tag_image">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://iri_sign_description/urdf/signs/dae/${tag_dae}.dae" scale="${tag_size} 1.0 ${tag_size}"/>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="joint_${name}_tag_link_to_${name}_tag_image" type="fixed">
+ <origin xyz="0 0 0" rpy="0 ${PI} -${PI/2}" />
+ <parent link="${name}_tag_link"/>
+ <child link="${name}_tag_image" />
+ </joint>
+
+ <gazebo reference="${name}_tag_image">
+ </gazebo>
+
+ <gazebo>
+ <static>true</static>
+ </gazebo>
+
+ <!--
+ <gazebo>
+ <plugin name="${name}_ground_truth" filename="libgazebo_ros_p3d.so">
+ <frameName>map</frameName>
+ <bodyName>${name}_base_link</bodyName>
+ <topicName>${name}_odom_ground_truth</topicName>
+ <updateRate>5.0</updateRate>
+ </plugin>
+ </gazebo>
+ -->
+
+ </xacro:macro>
+
+</root>
diff --git a/urdf/sample_box_example.xacro b/urdf/sample_box_example.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..44cb321eccf8ea9b889d9c5e753cf3e882f9908b
--- /dev/null
+++ b/urdf/sample_box_example.xacro
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<robot name="$(arg name)" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find iri_gazebo_set_material_plugin)/urdf/sample_box_macro.xacro" />
+
+ <xacro:sample_box name="$(arg name)"
+ gazebo_material="Gazebo/Green"
+ gazebo_material2="Gazebo/Red">
+ </xacro:sample_box>
+
+</robot>
diff --git a/urdf/sample_box_macro.xacro b/urdf/sample_box_macro.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..c653a12a505691d96da7f876edebde39bb407c01
--- /dev/null
+++ b/urdf/sample_box_macro.xacro
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:property name="PI" value="3.1415926535897931" />
+ <xacro:property name="box_width" value="0.5" />
+ <xacro:property name="box_depth" value="0.5" />
+ <xacro:property name="box_height" value="0.5" />
+
+ <xacro:macro name="sample_box"
+ params="name:=sample_box
+ gazebo_material:=Gazebo/Green
+ gazebo_material2:=Gazebo/Red">
+
+ <link name="${name}_base_footprint"/>
+
+ <link name="${name}_base_link">
+ <inertial>
+ <mass value="10"/>
+<!-- <origin xyz="0 0 ${box_height/2}" rpy="0 0 0"/> -->
+ <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
+ </inertial>
+ <visual>
+<!-- <origin xyz="0 0 ${box_height/2}" rpy="0 0 0"/> -->
+ <geometry>
+ <box size="${box_depth} ${box_width} ${box_height}"/>
+ </geometry>
+ </visual>
+ <collision>
+<!-- <origin xyz="0 0 ${box_height/2}" rpy="0 0 0"/> -->
+ <geometry>
+ <box size="${box_depth} ${box_width} ${box_height}"/>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="joint_${name}_base_footprint_to_${name}_base_link" type="fixed">
+ <origin xyz="0 0 ${box_height/2}" rpy="0 0 0" />
+ <parent link="${name}_base_footprint"/>
+ <child link="${name}_base_link" />
+ </joint>
+
+ <gazebo reference="${name}_base_link">
+
+ <visual>
+ <plugin name="${name}_set_material_plugin" filename="libiri_gazebo_set_material_plugin.so" >
+ <material>${gazebo_material}</material>
+ <material2>${gazebo_material2}</material2>
+ </plugin>
+ </visual>
+
+ </gazebo>
+
+ </xacro:macro>
+
+</root>