gazebo_rh_pub.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 #include <ros/ros.h>
18 #include <std_msgs/Float64.h>
19 
23 
24 void rhJointCallback(const std_msgs::Float64::ConstPtr& msg)
25 {
26  std_msgs::Float64 grip_joint_msg, grip_joint_msg_2;
27 
28  grip_joint_msg.data = msg->data;
29  grip_joint_msg_2.data = msg->data * (1.0 / 1.1);
30 
31  g_rh_r2_joint_pub.publish(grip_joint_msg_2);
32  g_rh_l1_joint_pub.publish(grip_joint_msg);
33  g_rh_l2_joint_pub.publish(grip_joint_msg_2);
34 }
35 
36 int main(int argc, char **argv)
37 {
38  ros::init(argc, argv, "gazebo_gripper_publisher");
39  ros::NodeHandle nh("~");
40 
41  g_rh_r2_joint_pub = nh.advertise<std_msgs::Float64>("/rh_p12_rn/rh_r2_position/command", 0);
42  g_rh_l1_joint_pub = nh.advertise<std_msgs::Float64>("/rh_p12_rn/rh_l1_position/command", 0);
43  g_rh_l2_joint_pub = nh.advertise<std_msgs::Float64>("/rh_p12_rn/rh_l2_position/command", 0);
44 
45  ros::Subscriber rh_joint_sub = nh.subscribe("/rh_p12_rn/rh_p12_rn_position/command", 5, rhJointCallback);
46 
47  ros::spin();
48 
49  return 0;
50 }
ros::Publisher g_rh_l1_joint_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)
ros::Publisher g_rh_l2_joint_pub
ROSCPP_DECL void spin(Spinner &spinner)
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher g_rh_r2_joint_pub
void rhJointCallback(const std_msgs::Float64::ConstPtr &msg)


rh_p12_rn_gazebo
Author(s): SCH , Zerom
autogenerated on Mon Jun 10 2019 14:42:11