thormang3_manipulation_demo.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 /*
18  * thormang3_manipulation_demo.cpp
19  *
20  * Created on: Jul 11, 2015
21  * Author: SCH
22  */
23 
24 #include <ros/ros.h>
25 #include <std_msgs/String.h>
26 #include <pthread.h>
27 
28 #include "robotis_controller_msgs/JointCtrlModule.h"
29 #include "thormang3_manipulation_module_msgs/KinematicsPose.h"
30 
35 
36 void demoCommandCallback(const std_msgs::String& msg)
37 {
38  if (msg.data == "ini_pose")
39  {
40  ROS_INFO("demo 1: go to initial pose");
41 
42  std_msgs::String demo_msg;
43  demo_msg.data = "ini_pose";
44 
45  g_base_ini_pose_pub.publish(demo_msg);
46  }
47  else if (msg.data == "set_mode")
48  {
49  ROS_INFO("demo 2: set manipulation control mode");
50 
51  std_msgs::String demo_msg;
52  demo_msg.data = "manipulation_module";
53 
54  g_enable_ctrl_module_pub.publish(demo_msg);
55  }
56  else if (msg.data == "base_pose")
57  {
58  ROS_INFO("demo 3: go to manipulation base pose");
59 
60  std_msgs::String demo_msgs;
61  demo_msgs.data = "ini_pose";
62 
63  g_manipulation_ini_pose_pub.publish(demo_msgs);
64  }
65  else if (msg.data == "right_arm")
66  {
67  ROS_INFO("demo 4: move right arm");
68 
69  thormang3_manipulation_module_msgs::KinematicsPose demo_msgs;
70 
71  demo_msgs.name = "right_arm";
72  demo_msgs.pose.position.x = 0.4;
73  demo_msgs.pose.position.y = -0.2;
74  demo_msgs.pose.position.z = 0.9;
75  demo_msgs.pose.orientation.w = 1.0;
76  demo_msgs.pose.orientation.x = 0.0;
77  demo_msgs.pose.orientation.y = 0.0;
78  demo_msgs.pose.orientation.z = 0.0;
79 
80  g_kinematics_msg_pub.publish(demo_msgs);
81  }
82  else if (msg.data == "left_arm")
83  {
84  ROS_INFO("demo 5: move left arm");
85 
86  thormang3_manipulation_module_msgs::KinematicsPose demo_msgs;
87 
88  demo_msgs.name = "left_arm_with_torso";
89  demo_msgs.pose.position.x = 0.4;
90  demo_msgs.pose.position.y = 0.2;
91  demo_msgs.pose.position.z = 0.9;
92  demo_msgs.pose.orientation.w = 1.0;
93  demo_msgs.pose.orientation.x = 0.0;
94  demo_msgs.pose.orientation.y = 0.0;
95  demo_msgs.pose.orientation.z = 0.0;
96 
97  g_kinematics_msg_pub.publish(demo_msgs);
98  }
99  else
100  {
101  ROS_INFO("there is no demo");
102  }
103 }
104 
105 int main(int argc, char **argv)
106 {
107  ros::init(argc, argv, "manipulation_demo_publisher");
108  ros::NodeHandle nh("~");
109 
110  g_base_ini_pose_pub = nh.advertise<std_msgs::String>("/robotis/base/ini_pose", 0);
111  g_enable_ctrl_module_pub = nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
112  g_kinematics_msg_pub = nh.advertise<thormang3_manipulation_module_msgs::KinematicsPose>("/robotis/manipulation/kinematics_pose_msg", 0);
113  g_manipulation_ini_pose_pub = nh.advertise<std_msgs::String>("/robotis/manipulation/ini_pose_msg", 0);
114 
115  ros::Subscriber demo_command_sub = nh.subscribe("/robotis/manipulation_demo/command", 5, demoCommandCallback);
116 
117  ROS_INFO("ROBOTIS THORMANG3 Manipulation Simple Demo");
118 
119  ros::spin();
120 
121  return 0;
122 }
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_kinematics_msg_pub
#define ROS_INFO(...)
ros::Publisher g_manipulation_ini_pose_pub
ros::Publisher g_enable_ctrl_module_pub
void demoCommandCallback(const std_msgs::String &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
ros::Publisher g_base_ini_pose_pub
int main(int argc, char **argv)


thormang3_manipulation_demo
Author(s): Zerom , SCH , Kayman , Jay Song
autogenerated on Mon Jun 10 2019 15:39:31