From edb57a895bc47313f1274ef6bf073a36462eefce Mon Sep 17 00:00:00 2001
From: Erdal Pekel <info@erdalpekel.de>
Date: Tue, 12 Mar 2019 05:13:36 -0700
Subject: [PATCH] Added simple box publisher for Gazebo
---
CMakeLists.txt | 4 ++-
README.md | 4 +++
launch/box.launch | 9 ++++++
models/box.xacro | 40 ++++++++++++++++++++++++
src/box_publisher.cpp | 71 +++++++++++++++++++++++++++++++++++++++++++
5 files changed, 127 insertions(+), 1 deletion(-)
create mode 100644 launch/box.launch
create mode 100644 models/box.xacro
create mode 100644 src/box_publisher.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3f59073..bc6fad6 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 0aaa6a3..fea7f65 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 0000000..5e57917
--- /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 0000000..4cfb076
--- /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 0000000..339f3f2
--- /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
--
GitLab