Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <algorithm>
00018 #include <vector>
00019
00020 #include <ros/ros.h>
00021
00022 #include <osrf_gear/Goal.h>
00023 #include <osrf_gear/LogicalCameraImage.h>
00024 #include <osrf_gear/Proximity.h>
00025 #include <sensor_msgs/JointState.h>
00026 #include <sensor_msgs/LaserScan.h>
00027 #include <std_msgs/Float32.h>
00028 #include <std_msgs/String.h>
00029 #include <std_srvs/Trigger.h>
00030 #include <trajectory_msgs/JointTrajectory.h>
00031
00032
00033
00035 void start_competition(ros::NodeHandle & node) {
00036
00037 ros::ServiceClient start_client =
00038 node.serviceClient<std_srvs::Trigger>("/ariac/start_competition");
00039
00040
00041 if (!start_client.exists()) {
00042 ROS_INFO("Waiting for the competition to be ready...");
00043 start_client.waitForExistence();
00044 ROS_INFO("Competition is now ready.");
00045 }
00046 ROS_INFO("Requesting competition start...");
00047 std_srvs::Trigger srv;
00048 start_client.call(srv);
00049 if (!srv.response.success) {
00050 ROS_ERROR_STREAM("Failed to start the competition: " << srv.response.message);
00051 } else {
00052 ROS_INFO("Competition started!");
00053 }
00054 }
00055
00056
00058 class MyCompetitionClass
00059 {
00060 public:
00061 explicit MyCompetitionClass(ros::NodeHandle & node)
00062 : current_score_(0), has_been_zeroed_(false)
00063 {
00064
00065 joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
00066 "/ariac/arm/command", 10);
00067
00068 }
00069
00071 void current_score_callback(const std_msgs::Float32::ConstPtr & msg) {
00072 if (msg->data != current_score_)
00073 {
00074 ROS_INFO_STREAM("Score: " << msg->data);
00075 }
00076 current_score_ = msg->data;
00077 }
00078
00080 void competition_state_callback(const std_msgs::String::ConstPtr & msg) {
00081 if (msg->data == "done" && competition_state_ != "done")
00082 {
00083 ROS_INFO("Competition ended.");
00084 }
00085 competition_state_ = msg->data;
00086 }
00087
00089 void goal_callback(const osrf_gear::Goal::ConstPtr & goal_msg) {
00090 ROS_INFO_STREAM("Received goal:\n" << *goal_msg);
00091 received_goals_.push_back(*goal_msg);
00092 }
00093
00094
00096 void joint_state_callback(
00097 const sensor_msgs::JointState::ConstPtr & joint_state_msg)
00098 {
00099 ROS_INFO_STREAM_THROTTLE(10,
00100 "Joint States (throttled to 0.1 Hz):\n" << *joint_state_msg);
00101
00102 current_joint_states_ = *joint_state_msg;
00103 if (!has_been_zeroed_) {
00104 has_been_zeroed_ = true;
00105 ROS_INFO("Sending arm to zero joint positions...");
00106 send_arm_to_zero_state();
00107 }
00108 }
00109
00110
00111
00113 void send_arm_to_zero_state() {
00114
00115 trajectory_msgs::JointTrajectory msg;
00116
00117 msg.joint_names = current_joint_states_.name;
00118
00119 msg.points.resize(1);
00120
00121
00122 msg.points[0].positions.resize(current_joint_states_.name.size(), 0.0);
00123
00124 msg.points[0].time_from_start = ros::Duration(0.001);
00125 ROS_INFO_STREAM("Sending command:\n" << msg);
00126 joint_trajectory_publisher_.publish(msg);
00127 }
00128
00129
00131 void logical_camera_callback(
00132 const osrf_gear::LogicalCameraImage::ConstPtr & image_msg)
00133 {
00134 ROS_INFO_STREAM_THROTTLE(10,
00135 "Logical camera: '" << image_msg->models.size() << "' objects.");
00136 }
00137
00139 void break_beam_callback(const osrf_gear::Proximity::ConstPtr & msg) {
00140 if (msg->object_detected) {
00141 ROS_INFO("Break beam triggered.");
00142 }
00143 }
00144
00145 private:
00146 std::string competition_state_;
00147 double current_score_;
00148 ros::Publisher joint_trajectory_publisher_;
00149 std::vector<osrf_gear::Goal> received_goals_;
00150 sensor_msgs::JointState current_joint_states_;
00151 bool has_been_zeroed_;
00152 };
00153
00154 void proximity_sensor_callback(const osrf_gear::Proximity::ConstPtr & msg) {
00155 if (msg->object_detected) {
00156 ROS_INFO("Proximity sensor triggered.");
00157 }
00158 }
00159
00160 void laser_profiler_callback(const sensor_msgs::LaserScan::ConstPtr & msg) {
00161 size_t number_of_valid_ranges = std::count_if(
00162 msg->ranges.begin(), msg->ranges.end(), std::isfinite<float>);
00163 if (number_of_valid_ranges > 0) {
00164 ROS_INFO_THROTTLE(1, "Laser profiler sees something.");
00165 }
00166 }
00167
00168
00169 int main(int argc, char ** argv) {
00170
00171 ros::init(argc, argv, "gear_example_node");
00172
00173 ros::NodeHandle node;
00174
00175
00176 MyCompetitionClass comp_class(node);
00177
00178
00179 ros::Subscriber current_score_subscriber = node.subscribe(
00180 "/ariac/current_score", 10,
00181 &MyCompetitionClass::current_score_callback, &comp_class);
00182
00183
00184 ros::Subscriber competition_state_subscriber = node.subscribe(
00185 "/ariac/competition_state", 10,
00186 &MyCompetitionClass::competition_state_callback, &comp_class);
00187
00188
00189
00190 ros::Subscriber goals_subscriber = node.subscribe(
00191 "/ariac/orders", 10,
00192 &MyCompetitionClass::goal_callback, &comp_class);
00193
00194
00195
00196 ros::Subscriber joint_state_subscriber = node.subscribe(
00197 "/ariac/joint_states", 10,
00198 &MyCompetitionClass::joint_state_callback, &comp_class);
00199
00200
00201
00202 ros::Subscriber proximity_sensor_subscriber = node.subscribe(
00203 "/ariac/proximity_sensor_1_changed", 10, proximity_sensor_callback);
00204
00205
00206
00207 ros::Subscriber break_beam_subscriber = node.subscribe(
00208 "/ariac/break_beam_1_changed", 10,
00209 &MyCompetitionClass::break_beam_callback, &comp_class);
00210
00211
00212 ros::Subscriber logical_camera_subscriber = node.subscribe(
00213 "/ariac/logical_camera_1", 10,
00214 &MyCompetitionClass::logical_camera_callback, &comp_class);
00215
00216
00217 ros::Subscriber laser_profiler_subscriber = node.subscribe(
00218 "/ariac/laser_profiler_1", 10, laser_profiler_callback);
00219
00220 ROS_INFO("Setup complete.");
00221 start_competition(node);
00222 ros::spin();
00223
00224 return 0;
00225 }
00226
00227