diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3f59073891b735b44f978dd6e0f296c15cf5412d..bc6fad654972261cc60cac3b1ae5c9467d077dc0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,5 +57,7 @@ include_directories(
)
add_executable(robot_control_node src/robot_control_node.cpp)
+add_executable(box_publisher_node src/box_publisher.cpp)
-target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
\ No newline at end of file
+target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
+target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
\ No newline at end of file
diff --git a/README.md b/README.md
index 0aaa6a3be6ecdccfc43d9575ceeb5616171a60f6..fea7f6553d958764228efa8ed0ccca7936c0964f 100644
--- a/README.md
+++ b/README.md
@@ -29,3 +29,7 @@ You can see the full explanation in my [blog post](https://erdalpekel.de/?p=55).
## Extension: _MoveIt!_ constraint-aware planning
This repository was extended with a ROS node that communicates with the _MoveIt!_ Planning Scene API. It makes sure that the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_simulation`) as _JSON_ files. You can read more about it in the accompanying [blog post](https://erdalpekel.de/?p=123).
+
+## Extension: Publishing a box at Panda's hand in _Gazebo_
+
+This repository was extended with a node that publishes a simple box object in the _Gazebo_ simulation at the hand of the robot. The position of this box will get updated as soon as the robot moves. You can read more about it in the accompanying [blog post](https://erdalpekel.de/?p=178).
\ No newline at end of file
diff --git a/launch/box.launch b/launch/box.launch
new file mode 100644
index 0000000000000000000000000000000000000000..5e57917241925b03d71628859d717a39d9e1d55c
--- /dev/null
+++ b/launch/box.launch
@@ -0,0 +1,9 @@
+<launch>
+ <param name="box_description" command="$(find xacro)/xacro --inorder $(find panda_simulation)/models/box.xacro"/>
+ <node name="box_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" args="robot_description:=box_description" />
+ <node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-param box_description -urdf -model box"/>
+
+
+ <!-- launch node -->
+ <node pkg="panda_simulation" type="box_publisher_node" name="box_publisher_node" />
+</launch>
\ No newline at end of file
diff --git a/models/box.xacro b/models/box.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..4cfb076b1bce798a603542f04de35a14eb42e018
--- /dev/null
+++ b/models/box.xacro
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="simple_box">
+ <link name="object_base_link">
+ </link>
+
+ <joint name="object_base_joint" type="fixed">
+ <parent link="object_base_link"/>
+ <child link="object_link"/>
+ <axis xyz="0 0 1" />
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ </joint>
+
+ <link name="object_link">
+ <inertial>
+ <origin xyz="0 0 0" />
+ <mass value="1.0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <box size="0.04 0.04 0.08" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0"/>
+ <geometry>
+ <box size="0.04 0.04 0.08" />
+ </geometry>
+ </collision>
+ </link>
+
+ <gazebo reference="object_base_link">
+ <gravity>0</gravity>
+ </gazebo>
+
+ <gazebo reference="object_link">
+ <material>Gazebo/Blue</material>
+ </gazebo>
+</robot>
\ No newline at end of file
diff --git a/src/box_publisher.cpp b/src/box_publisher.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..339f3f292f420a8e37c8124975e7fb5191948c3a
--- /dev/null
+++ b/src/box_publisher.cpp
@@ -0,0 +1,71 @@
+#include <gazebo_msgs/SetModelState.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+
+static const std::string PLANNING_GROUP_ARM = "panda_arm";
+static const double PANDA_ARM_TO_HAND_OFFSET = 0.12;
+static const double PANDA_HAND_TO_FINGER_OFFSET = 0.04;
+ros::Publisher gazebo_model_state_pub;
+robot_model::RobotModelPtr kinematic_model;
+robot_state::RobotStatePtr kinematic_state;
+
+void jointStatesCallback(const sensor_msgs::JointState &joint_states_current)
+{
+ const robot_state::JointModelGroup *joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
+ const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
+
+ std::vector<double> joint_states;
+ for (size_t i = 0; i < joint_states_current.position.size() - 2; ++i)
+ {
+ joint_states.push_back(joint_states_current.position[i + 2]);
+ }
+ kinematic_state->setJointGroupPositions(joint_model_group, joint_states);
+
+ const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");
+
+ double end_effector_z_offset = PANDA_ARM_TO_HAND_OFFSET + PANDA_HAND_TO_FINGER_OFFSET;
+ Eigen::Affine3d tmp_transform(Eigen::Translation3d(Eigen::Vector3d(0.0, 0.0, end_effector_z_offset)));
+
+ Eigen::Affine3d newState = end_effector_state * tmp_transform;
+
+ geometry_msgs::Pose pose;
+ pose.position.x = newState.translation().x();
+ pose.position.y = newState.translation().y();
+ pose.position.z = newState.translation().z();
+
+ Eigen::Quaterniond quat(newState.rotation());
+ pose.orientation.w = quat.w();
+ pose.orientation.x = quat.x();
+ pose.orientation.y = quat.y();
+ pose.orientation.z = quat.z();
+
+ ROS_DEBUG_STREAM("translation" << std::endl << newState.translation());
+ ROS_DEBUG_STREAM("rotation" << std::endl << newState.rotation());
+
+ gazebo_msgs::ModelState model_state;
+ // This string results from the spawn_urdf call in the box.launch file argument: -model box
+ model_state.model_name = std::string("box");
+ model_state.pose = pose;
+ model_state.reference_frame = std::string("world");
+
+ gazebo_model_state_pub.publish(model_state);
+}
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "box_publisher_node");
+ ros::NodeHandle node_handle;
+
+ ros::Subscriber joint_states_sub = node_handle.subscribe("/joint_states", 1, jointStatesCallback);
+
+ robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
+ kinematic_model = robot_model_loader.getModel();
+ kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
+
+ gazebo_model_state_pub = node_handle.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 1);
+
+ ros::spin();
+ return 0;
+}
\ No newline at end of file