int main(int argc, char* argv[]) { // These following lines are used to Make sure that command lline args are correct if(argc<5) // if number of args are less than 5 { cerr << "Usage: " << argv[0] << " <socket> <left_motor_port> <right_motor_port> <sensor_port> <hz>" << endl; return 1; } int milliseconds = 100; if(argc==6) milliseconds = 1000/atoi(argv[5]); // cout<<"milliseconds"<<milliseconds; string left_motor_port(argv[2]); string right_motor_port(argv[3]); string sensor_port(argv[4]); // if(left_motor_port<1||left_motor_port>4||right_motor_port<1||right_motor_port>4||left_motor_port==right_motor_port) // { // cerr << "Invalid motor port numbers. Must be 1, 2, 3 or 4 and distinct." << endl; // return 1; // } // TODO: Check if both are of same type left_motor = motor(left_motor_port); right_motor = motor(right_motor_port); s = sensor(sensor_port); if(s.type()!="ev3-uart-30") // sensor object s will not be used hereafter { cerr << "Invalid sensor type. Must be EV3 ultrasonic. Given sensor is of type " << s.type() << endl; return 1; } //--------------------------------------------------------------------------------------------------------------------------------------------- // TODO: Check if both were initialised left_motor.reset(); left_motor.set_position(0); left_motor.set_run_mode("time");// changed from forever mode to time left_motor.set_stop_mode("brake"); left_motor.set_regulation_mode("on"); right_motor.reset(); right_motor.set_position(0); right_motor.set_run_mode("time"); right_motor.set_stop_mode("brake"); right_motor.set_regulation_mode("on"); nav_msgs::Odometry odom_msg;// msg object for the publisher ros::Publisher odom_pub("/robot3/odom"/*topic name*/, &odom_msg); nh.advertise(odom_pub); // advertises that odom_pub is the publisher name tf::TransformBroadcaster odom_broadcaster; //broadcaster msg for the odometry tf::TransformBroadcaster scan_broadcaster; // broadcaster for the ultrasonic sensor nh.subscribe(pose_sub); ros::Time current_time, last_time; current_time = nh.now(); last_time = nh.now(); nh.initNode(argv[1]); odom_broadcaster.init(nh); //nh.advertiseService(server1); nh.advertiseService(server2); while(!nh.connected()) {nh.spinOnce();} // JUST TO KNOW IF THE NODE IS ALIVE cout<<"1. Gostraight service\n2.Trace an arc service"<<endl; while(1) { // check for incoming messages current_time = nh.now(); geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(t); // odom_quat stores Quaternion cretaed from yaw // cout<<" Calculated quat: "<<endl; //first, we'll publish the transform over tf geometry_msgs::TransformStamped odom_trans;//message object odom_trans.header.stamp = current_time; odom_trans.header.frame_id = map_name;//map_name is just given as map const char base_link_name[18] = "/robot3/base_link"; odom_trans.child_frame_id = base_link_name; // cout<<" constructed header"<<endl; // loading the message object with data odom_trans.transform.translation.x = x; odom_trans.transform.translation.y = y; odom_trans.transform.translation.z = 0.0;// as we are dealing with xy plane odom_trans.transform.rotation = odom_quat;// this is quaternion created from yaw angle t // cout<<" Constructed full message"<<endl; //send the transform odom_broadcaster.sendTransform(odom_trans); //next, we'll publish the odometry message over ROS odom_msg.header.stamp = current_time; odom_msg.header.frame_id = map_name; //set the position odom_msg.pose.pose.position.x = x; odom_msg.pose.pose.position.y = y; odom_msg.pose.pose.position.z = 0.0; odom_msg.pose.pose.orientation = odom_quat; //set the velocity odom_msg.child_frame_id = base_link_name; odom_msg.twist.twist.linear.x = vx; odom_msg.twist.twist.linear.y = 0; odom_msg.twist.twist.angular.z = wt; // cout<<" sizeof "<<sizeof(odom_msg)<<endl; //publish the message odom_pub.publish(&odom_msg); cout<<"Services are being advertised. Waiting for request"<<endl; nh.spinOnce(); sleep(1); } return 0; }
void motor_action(motor &dev) { char c = 0; int new_value = 0; std::string answer; bool running = false; do { cout << endl << "*** " << dev.driver_name() << " motor (" << dev.port_name() << ") actions ***" << endl << endl << "(i)nfo" << endl << "(c)ommand" << endl << "st(o)p command [" << dev.stop_command() << "]" << endl << "speed r(e)gulation [" << dev.speed_regulation_enabled() << "]" << endl; if (dev.speed_regulation_enabled()==dev.speed_regulation_on) cout << "speed (s)etpoint (" << dev.speed_sp() << ")" << endl; else cout << "duty cycle (s)etpoint (" << dev.duty_cycle_sp() << ")" << endl; cout << "(p)osition setpoint (" << dev.position_sp() << ")" << endl << "ramp (u)p setpoint (" << dev.ramp_up_sp() << ")" << endl << "ramp (d)own setpoint (" << dev.ramp_down_sp() << ")" << endl << "(t)ime setpoint (" << dev.time_sp() << ")" << endl << endl << "(0) reset position" << endl << endl << "(b)ack" << endl << endl << "Choice: "; cin >> c; switch (c) { case 'i': cout << endl; //~autogen cpp_generic_report_status classes.motor>currentClass cout << " Commands: "; try { cout << dev.commands() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Count Per Rot: "; try { cout << dev.count_per_rot() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Driver Name: "; try { cout << dev.driver_name() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Duty Cycle: "; try { cout << dev.duty_cycle() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Duty Cycle SP: "; try { cout << dev.duty_cycle_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Encoder Polarity: "; try { cout << dev.encoder_polarity() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Polarity: "; try { cout << dev.polarity() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Port Name: "; try { cout << dev.port_name() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position: "; try { cout << dev.position() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position P: "; try { cout << dev.position_p() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position I: "; try { cout << dev.position_i() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position D: "; try { cout << dev.position_d() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position SP: "; try { cout << dev.position_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed: "; try { cout << dev.speed() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed SP: "; try { cout << dev.speed_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Ramp Up SP: "; try { cout << dev.ramp_up_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Ramp Down SP: "; try { cout << dev.ramp_down_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed Regulation Enabled: "; try { cout << dev.speed_regulation_enabled() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed Regulation P: "; try { cout << dev.speed_regulation_p() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed Regulation I: "; try { cout << dev.speed_regulation_i() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed Regulation D: "; try { cout << dev.speed_regulation_d() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " State: "; try { cout << dev.state() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Stop Command: "; try { cout << dev.stop_command() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Stop Commands: "; try { cout << dev.stop_commands() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Time SP: "; try { cout << dev.time_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } //~autogen cout << endl; break; case 'c': cout << "command " << dev.commands() << ": "; cin >> answer; dev.set_command(answer); cout << endl; break; case 'o': cout << "stop command " << dev.stop_commands() << ": "; cin >> answer; dev.set_stop_command(answer); cout << endl; break; case 'e': cout << "speed regulation (off, on): "; cin >> answer; dev.set_speed_regulation_enabled(answer); cout << endl; break; case 's': if (dev.speed_regulation_enabled()==dev.speed_regulation_on) { cout << "speed: "; cin >> new_value; dev.set_speed_sp(new_value); cout << endl; } else { cout << "duty cycle: "; cin >> new_value; dev.set_duty_cycle_sp(new_value); cout << endl; } break; case 'p': cout << "position: "; cin >> new_value; dev.set_position_sp(new_value); cout << endl; break; case 'u': cout << "ramp up: "; cin >> new_value; dev.set_ramp_up_sp(new_value); cout << endl; break; case 'd': cout << "ramp down: "; cin >> new_value; dev.set_ramp_down_sp(new_value); cout << endl; break; case 't': cout << "time: "; cin >> new_value; dev.set_time_sp(new_value); cout << endl; break; case '0': dev.set_position(0); break; }
void motor_action(motor &dev) { char c = 0; int new_value = 0; std::string answer; bool running = false; do { cout << endl << "*** " << dev.driver_name() << " motor (" << dev.address() << ") actions ***" << endl << endl << "(i)nfo" << endl << "(c)ommand" << endl << "st(o)p command [" << dev.stop_action() << "]" << endl << "speed (s)etpoint (" << dev.speed_sp() << ")" << endl; cout << "(p)osition setpoint (" << dev.position_sp() << ")" << endl << "ramp (u)p setpoint (" << dev.ramp_up_sp() << ")" << endl << "ramp (d)own setpoint (" << dev.ramp_down_sp() << ")" << endl << "(t)ime setpoint (" << dev.time_sp() << ")" << endl << endl << "(0) reset position" << endl << endl << "(b)ack" << endl << endl << "Choice: "; cin >> c; switch (c) { case 'i': cout << endl; //~autogen generic_report_status classes.motor>currentClass cout << " Address: "; try { cout << dev.address() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Commands: "; try { cout << dev.commands() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Count Per Rot: "; try { cout << dev.count_per_rot() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Count Per M: "; try { cout << dev.count_per_m() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Driver Name: "; try { cout << dev.driver_name() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Duty Cycle: "; try { cout << dev.duty_cycle() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Duty Cycle SP: "; try { cout << dev.duty_cycle_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Full Travel Count: "; try { cout << dev.full_travel_count() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Polarity: "; try { cout << dev.polarity() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position: "; try { cout << dev.position() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position P: "; try { cout << dev.position_p() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position I: "; try { cout << dev.position_i() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position D: "; try { cout << dev.position_d() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Position SP: "; try { cout << dev.position_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Max Speed: "; try { cout << dev.max_speed() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed: "; try { cout << dev.speed() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed SP: "; try { cout << dev.speed_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Ramp Up SP: "; try { cout << dev.ramp_up_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Ramp Down SP: "; try { cout << dev.ramp_down_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed P: "; try { cout << dev.speed_p() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed I: "; try { cout << dev.speed_i() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Speed D: "; try { cout << dev.speed_d() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " State: "; try { cout << dev.state() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Stop Action: "; try { cout << dev.stop_action() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Stop Actions: "; try { cout << dev.stop_actions() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } cout << " Time SP: "; try { cout << dev.time_sp() << endl; } catch(...) { cout << "[" << strerror(errno) << "]" << endl; } //~autogen cout << endl; break; case 'c': cout << "command " << dev.commands() << ": "; cin >> answer; dev.set_command(answer); cout << endl; break; case 'o': cout << "stop command " << dev.stop_actions() << ": "; cin >> answer; dev.set_stop_action(answer); cout << endl; break; case 's': cout << "speed: "; cin >> new_value; dev.set_speed_sp(new_value); cout << endl; break; case 'p': cout << "position: "; cin >> new_value; dev.set_position_sp(new_value); cout << endl; break; case 'u': cout << "ramp up: "; cin >> new_value; dev.set_ramp_up_sp(new_value); cout << endl; break; case 'd': cout << "ramp down: "; cin >> new_value; dev.set_ramp_down_sp(new_value); cout << endl; break; case 't': cout << "time: "; cin >> new_value; dev.set_time_sp(new_value); cout << endl; break; case '0': dev.set_position(0); break; } } while (c != 'b'); }