Select Git revision
Forked from
CeTI / ROS / ROS Packages / sample_applications
Source project has a limited visibility.
box_publisher.cpp 2.73 KiB
#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;
}