00001
00002 #include <ros/ros.h>
00003 #include <dynamic_reconfigure/server.h>
00004 #include <cob_teleop_cob4/cob_teleop_cob4Config.h>
00005
00006
00007 #include <sensor_msgs/JoyFeedbackArray.h>
00008 #include <geometry_msgs/Twist.h>
00009 #include <geometry_msgs/Twist.h>
00010 #include <geometry_msgs/Twist.h>
00011 #include <brics_actuator/JointVelocities.h>
00012 #include <brics_actuator/JointVelocities.h>
00013 #include <geometry_msgs/Twist.h>
00014 #include <brics_actuator/JointVelocities.h>
00015 #include <geometry_msgs/Twist.h>
00016 #include <brics_actuator/JointVelocities.h>
00017 #include <brics_actuator/JointVelocities.h>
00018 #include <sensor_msgs/Joy.h>
00019
00020
00021 #include <cob_teleop_cob4_common.cpp>
00022
00023
00024 class cob_teleop_cob4_ros
00025 {
00026 public:
00027 ros::NodeHandle n_;
00028 ros::NodeHandle np_;
00029
00030 dynamic_reconfigure::Server<cob_teleop_cob4::cob_teleop_cob4Config> server;
00031 dynamic_reconfigure::Server<cob_teleop_cob4::cob_teleop_cob4Config>::CallbackType f;
00032
00033 ros::Publisher joy_feedback_;
00034 ros::Publisher base_controller_command_;
00035 ros::Publisher arm_cart_left_;
00036 ros::Publisher arm_cart_right_;
00037 ros::Publisher arm_joint_right_;
00038 ros::Publisher arm_joint_left_;
00039 ros::Publisher head_controller_command_;
00040 ros::Publisher sensorring_controller_command_;
00041 ros::Publisher torso_controller_command_;
00042 ros::Publisher gripper_left_;
00043 ros::Publisher gripper_right_;
00044 ros::Subscriber joy_;
00045
00046 cob_teleop_cob4_data component_data_;
00047 cob_teleop_cob4_config component_config_;
00048 cob_teleop_cob4_impl component_implementation_;
00049
00050 cob_teleop_cob4_ros() : np_("~")
00051 {
00052 f = boost::bind(&cob_teleop_cob4_ros::configure_callback, this, _1, _2);
00053 server.setCallback(f);
00054
00055
00056 joy_feedback_ = n_.advertise<sensor_msgs::JoyFeedbackArray>("joy_feedback", 1);
00057 base_controller_command_ = n_.advertise<geometry_msgs::Twist>("base_controller_command", 1);
00058 arm_cart_left_ = n_.advertise<geometry_msgs::Twist>("arm_cart_left", 1);
00059 arm_cart_right_ = n_.advertise<geometry_msgs::Twist>("arm_cart_right", 1);
00060 arm_joint_right_ = n_.advertise<brics_actuator::JointVelocities>("arm_joint_right", 1);
00061 arm_joint_left_ = n_.advertise<brics_actuator::JointVelocities>("arm_joint_left", 1);
00062 head_controller_command_ = n_.advertise<geometry_msgs::Twist>("head_controller_command", 1);
00063 sensorring_controller_command_ = n_.advertise<brics_actuator::JointVelocities>("sensorring_controller_command", 1);
00064 torso_controller_command_ = n_.advertise<geometry_msgs::Twist>("torso_controller_command", 1);
00065 gripper_left_ = n_.advertise<brics_actuator::JointVelocities>("gripper_left", 1);
00066 gripper_right_ = n_.advertise<brics_actuator::JointVelocities>("gripper_right", 1);
00067 joy_ = n_.subscribe("joy", 1, &cob_teleop_cob4_ros::topicCallback_joy, this);
00068
00069 np_.param("button_deadman", component_config_.button_deadman, (int)11);
00070 np_.param("base_max_linear", component_config_.base_max_linear, (double)0.5);
00071 np_.param("base_max_angular", component_config_.base_max_angular, (double)1.5);
00072 np_.param("torso_max_angular", component_config_.torso_max_angular, (double)0.2);
00073 np_.param("head_max_angular", component_config_.head_max_angular, (double)0.3);
00074 np_.param("sensor_ring_max_angular", component_config_.sensor_ring_max_angular, (double)0.1);
00075 np_.param("arm_joint_velocity_max", component_config_.arm_joint_velocity_max, (double)0.3);
00076 np_.param("arm_cartesian_max_linear", component_config_.arm_cartesian_max_linear, (double)0.1);
00077 np_.param("arm_cartesian_max_angular", component_config_.arm_cartesian_max_angular, (double)0.1);
00078 np_.param("gripper_max_velocity", component_config_.gripper_max_velocity, (double)0.1);
00079 np_.param("base_x", component_config_.base_x, (int)1);
00080 np_.param("base_y", component_config_.base_y, (int)0);
00081 np_.param("base_yaw", component_config_.base_yaw, (int)2);
00082 np_.param("arm_x", component_config_.arm_x, (int)0);
00083 np_.param("arm_y", component_config_.arm_y, (int)1);
00084 np_.param("arm_yaw", component_config_.arm_yaw, (int)2);
00085 np_.param("arm_pitch_up", component_config_.arm_pitch_up, (int)4);
00086 np_.param("arm_pitch_down", component_config_.arm_pitch_down, (int)6);
00087 np_.param("arm_roll_right_and_ellbow", component_config_.arm_roll_right_and_ellbow, (int)5);
00088 np_.param("arm_roll_left_and_ellbow", component_config_.arm_roll_left_and_ellbow, (int)7);
00089 np_.param("arm_z_up", component_config_.arm_z_up, (int)12);
00090 np_.param("arm_z_down", component_config_.arm_z_down, (int)14);
00091 np_.param("gripper_open", component_config_.gripper_open, (int)15);
00092 np_.param("gripper_close", component_config_.gripper_close, (int)13);
00093 np_.param("arm_joint_up", component_config_.arm_joint_up, (int)4);
00094 np_.param("arm_joint_down", component_config_.arm_joint_down, (int)6);
00095 np_.param("arm_joint_left", component_config_.arm_joint_left, (int)7);
00096 np_.param("arm_joint_right", component_config_.arm_joint_right, (int)5);
00097 np_.param("arm_joint_12", component_config_.arm_joint_12, (int)15);
00098 np_.param("arm_joint_34", component_config_.arm_joint_34, (int)14);
00099 np_.param("arm_joint_56", component_config_.arm_joint_56, (int)13);
00100 np_.param("arm_joint_7_gripper", component_config_.arm_joint_7_gripper, (int)12);
00101 np_.param("axis_runfactor", component_config_.axis_runfactor, (int)9);
00102 np_.param("button_safety_override", component_config_.button_safety_override, (int)9);
00103 np_.param("button_init_recover", component_config_.button_init_recover, (int)3);
00104 np_.param("button_mode_switch", component_config_.button_mode_switch, (int)0);
00105 np_.param("torso_roll", component_config_.torso_roll, (int)0);
00106 np_.param("torso_pitch", component_config_.torso_pitch, (int)1);
00107 np_.param("torso_yaw_left", component_config_.torso_yaw_left, (int)15);
00108 np_.param("torso_yaw_right", component_config_.torso_yaw_right, (int)13);
00109 np_.param("sensorring_yaw_left", component_config_.sensorring_yaw_left, (int)4);
00110 np_.param("sensorring_yaw_right", component_config_.sensorring_yaw_right, (int)6);
00111 np_.param("head_roll", component_config_.head_roll, (int)2);
00112 np_.param("head_pitch", component_config_.head_pitch, (int)3);
00113 np_.param("head_yaw_left", component_config_.head_yaw_left, (int)7);
00114 np_.param("head_yaw_right", component_config_.head_yaw_right, (int)5);
00115 np_.param("head_home", component_config_.head_home, (int)4);
00116 np_.param("arm_left_home", component_config_.arm_left_home, (int)7);
00117 np_.param("arm_right_home", component_config_.arm_right_home, (int)5);
00118 np_.param("torso_home", component_config_.torso_home, (int)6);
00119 np_.param("sensorring_home", component_config_.sensorring_home, (int)12);
00120 np_.param("gripper_left_home", component_config_.gripper_left_home, (int)15);
00121 np_.param("gripper_right_home", component_config_.gripper_right_home, (int)13);
00122 np_.param("base_home", component_config_.base_home, (int)14);
00123 if(np_.hasParam("arm_left_uri"))
00124 np_.getParam("arm_left_uri", component_config_.arm_left_uri);
00125 else
00126 ROS_ERROR("Parameter arm_left_uri not set");
00127 if(np_.hasParam("components"))
00128 np_.getParam("components", component_config_.components);
00129 else
00130 ROS_ERROR("Parameter components not set");
00131 np_.param("home_time", component_config_.home_time, (double)5.0);
00132 np_.param("stop_time", component_config_.stop_time, (double)0.8);
00133 if(np_.hasParam("arm_right_uri"))
00134 np_.getParam("arm_right_uri", component_config_.arm_right_uri);
00135 else
00136 ROS_ERROR("Parameter arm_right_uri not set");
00137 if(np_.hasParam("led_mode"))
00138 np_.getParam("led_mode", component_config_.led_mode);
00139 else
00140 ROS_ERROR("Parameter led_mode not set");
00141 np_.param("gripper_1", component_config_.gripper_1, (int)3);
00142 np_.param("gripper_2", component_config_.gripper_2, (int)4);
00143 np_.param("gripper_max_angular", component_config_.gripper_max_angular, (double)0.2);
00144 }
00145 void topicCallback_joy(const sensor_msgs::Joy::ConstPtr& msg)
00146 {
00147 component_data_.in_joy = *msg;
00148 }
00149
00150 void configure_callback(cob_teleop_cob4::cob_teleop_cob4Config &config, uint32_t level)
00151 {
00152 component_config_.button_deadman = config.button_deadman;
00153 component_config_.base_max_linear = config.base_max_linear;
00154 component_config_.base_max_angular = config.base_max_angular;
00155 component_config_.torso_max_angular = config.torso_max_angular;
00156 component_config_.head_max_angular = config.head_max_angular;
00157 component_config_.sensor_ring_max_angular = config.sensor_ring_max_angular;
00158 component_config_.arm_joint_velocity_max = config.arm_joint_velocity_max;
00159 component_config_.arm_cartesian_max_linear = config.arm_cartesian_max_linear;
00160 component_config_.arm_cartesian_max_angular = config.arm_cartesian_max_angular;
00161 component_config_.gripper_max_velocity = config.gripper_max_velocity;
00162 component_config_.base_x = config.base_x;
00163 component_config_.base_y = config.base_y;
00164 component_config_.base_yaw = config.base_yaw;
00165 component_config_.arm_x = config.arm_x;
00166 component_config_.arm_y = config.arm_y;
00167 component_config_.arm_yaw = config.arm_yaw;
00168 component_config_.arm_pitch_up = config.arm_pitch_up;
00169 component_config_.arm_pitch_down = config.arm_pitch_down;
00170 component_config_.arm_roll_right_and_ellbow = config.arm_roll_right_and_ellbow;
00171 component_config_.arm_roll_left_and_ellbow = config.arm_roll_left_and_ellbow;
00172 component_config_.arm_z_up = config.arm_z_up;
00173 component_config_.arm_z_down = config.arm_z_down;
00174 component_config_.gripper_open = config.gripper_open;
00175 component_config_.gripper_close = config.gripper_close;
00176 component_config_.arm_joint_up = config.arm_joint_up;
00177 component_config_.arm_joint_down = config.arm_joint_down;
00178 component_config_.arm_joint_left = config.arm_joint_left;
00179 component_config_.arm_joint_right = config.arm_joint_right;
00180 component_config_.arm_joint_12 = config.arm_joint_12;
00181 component_config_.arm_joint_34 = config.arm_joint_34;
00182 component_config_.arm_joint_56 = config.arm_joint_56;
00183 component_config_.arm_joint_7_gripper = config.arm_joint_7_gripper;
00184 component_config_.axis_runfactor = config.axis_runfactor;
00185 component_config_.button_safety_override = config.button_safety_override;
00186 component_config_.button_init_recover = config.button_init_recover;
00187 component_config_.button_mode_switch = config.button_mode_switch;
00188 component_config_.torso_roll = config.torso_roll;
00189 component_config_.torso_pitch = config.torso_pitch;
00190 component_config_.torso_yaw_left = config.torso_yaw_left;
00191 component_config_.torso_yaw_right = config.torso_yaw_right;
00192 component_config_.sensorring_yaw_left = config.sensorring_yaw_left;
00193 component_config_.sensorring_yaw_right = config.sensorring_yaw_right;
00194 component_config_.head_roll = config.head_roll;
00195 component_config_.head_pitch = config.head_pitch;
00196 component_config_.head_yaw_left = config.head_yaw_left;
00197 component_config_.head_yaw_right = config.head_yaw_right;
00198 component_config_.head_home = config.head_home;
00199 component_config_.arm_left_home = config.arm_left_home;
00200 component_config_.arm_right_home = config.arm_right_home;
00201 component_config_.torso_home = config.torso_home;
00202 component_config_.sensorring_home = config.sensorring_home;
00203 component_config_.gripper_left_home = config.gripper_left_home;
00204 component_config_.gripper_right_home = config.gripper_right_home;
00205 component_config_.base_home = config.base_home;
00206 component_config_.home_time = config.home_time;
00207 component_config_.stop_time = config.stop_time;
00208 component_config_.gripper_1 = config.gripper_1;
00209 component_config_.gripper_2 = config.gripper_2;
00210 component_config_.gripper_max_angular = config.gripper_max_angular;
00211 }
00212
00213 void configure()
00214 {
00215 component_implementation_.configure(component_config_);
00216 }
00217
00218 void activate_all_output()
00219 {
00220 component_data_.out_joy_feedback_active = true;
00221 component_data_.out_base_controller_command_active = true;
00222 component_data_.out_arm_cart_left_active = true;
00223 component_data_.out_arm_cart_right_active = true;
00224 component_data_.out_arm_joint_right_active = true;
00225 component_data_.out_arm_joint_left_active = true;
00226 component_data_.out_head_controller_command_active = true;
00227 component_data_.out_sensorring_controller_command_active = true;
00228 component_data_.out_torso_controller_command_active = true;
00229 component_data_.out_gripper_left_active = true;
00230 component_data_.out_gripper_right_active = true;
00231 }
00232
00233 void update()
00234 {
00235 activate_all_output();
00236 component_implementation_.update(component_data_, component_config_);
00237 if (component_data_.out_joy_feedback_active)
00238 joy_feedback_.publish(component_data_.out_joy_feedback);
00239 if (component_data_.out_base_controller_command_active)
00240 base_controller_command_.publish(component_data_.out_base_controller_command);
00241 if (component_data_.out_arm_cart_left_active)
00242 arm_cart_left_.publish(component_data_.out_arm_cart_left);
00243 if (component_data_.out_arm_cart_right_active)
00244 arm_cart_right_.publish(component_data_.out_arm_cart_right);
00245 if (component_data_.out_arm_joint_right_active)
00246 arm_joint_right_.publish(component_data_.out_arm_joint_right);
00247 if (component_data_.out_arm_joint_left_active)
00248 arm_joint_left_.publish(component_data_.out_arm_joint_left);
00249 if (component_data_.out_head_controller_command_active)
00250 head_controller_command_.publish(component_data_.out_head_controller_command);
00251 if (component_data_.out_sensorring_controller_command_active)
00252 sensorring_controller_command_.publish(component_data_.out_sensorring_controller_command);
00253 if (component_data_.out_torso_controller_command_active)
00254 torso_controller_command_.publish(component_data_.out_torso_controller_command);
00255 if (component_data_.out_gripper_left_active)
00256 gripper_left_.publish(component_data_.out_gripper_left);
00257 if (component_data_.out_gripper_right_active)
00258 gripper_right_.publish(component_data_.out_gripper_right);
00259 }
00260 };
00261
00262 int main(int argc, char** argv)
00263 {
00264
00265 ros::init(argc, argv, "cob_teleop_cob4");
00266
00267 cob_teleop_cob4_ros node;
00268 node.configure();
00269
00270 ros::Rate loop_rate(50.0);
00271
00272 while(node.n_.ok())
00273 {
00274 node.update();
00275 loop_rate.sleep();
00276 ros::spinOnce();
00277 }
00278
00279 return 0;
00280 }