Added simple box publisher for Gazebo

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})
+target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
\ No newline at end of file

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

launch/box.launch

+<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

models/box.xacro

+<?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

src/box_publisher.cpp

+#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