25 #include <std_msgs/String.h> 28 #include "robotis_controller_msgs/JointCtrlModule.h" 29 #include "thormang3_manipulation_module_msgs/KinematicsPose.h" 38 if (msg.data ==
"ini_pose")
40 ROS_INFO(
"demo 1: go to initial pose");
42 std_msgs::String demo_msg;
43 demo_msg.data =
"ini_pose";
45 g_base_ini_pose_pub.
publish(demo_msg);
47 else if (msg.data ==
"set_mode")
49 ROS_INFO(
"demo 2: set manipulation control mode");
51 std_msgs::String demo_msg;
52 demo_msg.data =
"manipulation_module";
54 g_enable_ctrl_module_pub.
publish(demo_msg);
56 else if (msg.data ==
"base_pose")
58 ROS_INFO(
"demo 3: go to manipulation base pose");
60 std_msgs::String demo_msgs;
61 demo_msgs.data =
"ini_pose";
63 g_manipulation_ini_pose_pub.
publish(demo_msgs);
65 else if (msg.data ==
"right_arm")
69 thormang3_manipulation_module_msgs::KinematicsPose demo_msgs;
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;
80 g_kinematics_msg_pub.
publish(demo_msgs);
82 else if (msg.data ==
"left_arm")
86 thormang3_manipulation_module_msgs::KinematicsPose demo_msgs;
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;
97 g_kinematics_msg_pub.
publish(demo_msgs);
105 int main(
int argc,
char **argv)
107 ros::init(argc, argv,
"manipulation_demo_publisher");
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);
117 ROS_INFO(
"ROBOTIS THORMANG3 Manipulation Simple Demo");
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
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)
ros::Publisher g_base_ini_pose_pub
int main(int argc, char **argv)