18 #include <sensor_msgs/JointState.h> 25 sensor_msgs::JointState present_msg;
27 for (
int index = 0; index < msg->name.size(); index++)
29 present_msg.name.push_back(msg->name[index]);
30 present_msg.position.push_back(msg->position[index]);
32 if (present_msg.name[index] ==
"rh_p12_rn")
34 present_msg.name.push_back(
"rh_r2");
35 present_msg.position.push_back(present_msg.position[index] * (1.0 / 1.1));
36 present_msg.name.push_back(
"rh_l1");
37 present_msg.position.push_back(present_msg.position[index]);
38 present_msg.name.push_back(
"rh_l2");
39 present_msg.position.push_back(present_msg.position[index] * (1.0 / 1.1));
42 g_present_joint_states_pub.
publish(present_msg);
47 sensor_msgs::JointState goal_msg;
49 for (
int index = 0; index < msg->name.size(); index++)
51 goal_msg.name.push_back(msg->name[index]);
52 goal_msg.position.push_back(msg->position[index]);
54 if (goal_msg.name[index] ==
"rh_p12_rn")
56 goal_msg.name.push_back(
"rh_r2");
57 goal_msg.position.push_back(goal_msg.position[index] * (1.0 / 1.1));
58 goal_msg.name.push_back(
"rh_l1");
59 goal_msg.position.push_back(goal_msg.position[index]);
60 goal_msg.name.push_back(
"rh_l2");
61 goal_msg.position.push_back(goal_msg.position[index] * (1.0 / 1.1));
64 g_goal_joint_states_pub.
publish(goal_msg);
67 int main(
int argc,
char **argv)
69 ros::init(argc, argv,
"rviz_rh_p12_rn_publisher");
72 g_present_joint_states_pub = nh.
advertise<sensor_msgs::JointState>(
"/robotis/rh_p12_rn/present_joint_states", 0);
73 g_goal_joint_states_pub = nh.
advertise<sensor_msgs::JointState>(
"/robotis/rh_p12_rn/goal_joint_states", 0);
ros::Publisher g_goal_joint_states_pub
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
void goalJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void presentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
ros::Publisher g_present_joint_states_pub
int main(int argc, char **argv)