//----------------------------------------------------------------------------------------------------------------------------------------
void svcCallback_Trace_arc(const ev3_services::Trace_arc::Request &req , ev3_services::Trace_arc::Response &res){ // The callback function
//float vx, wt, vl, vr; // LOCAL VARIABLES DEFINED AGAIn
speed = R*deg2rad;
//req.time*=1000;//to convert secs to millisecs But this line shows an error so Commented it
res.vl= (req.vx+L/2*req.wt)/speed; // /speed was taken out
res.vr= (req.vx-L/2*req.wt)/speed;
if(res.vl!=0)
{
left_motor.set_pulses_per_second_setpoint(res.vl);
left_motor.set_time_setpoint(req.time);// time is in milliseconds
left_motor.run(true);
}
else
{
left_motor.set_pulses_per_second_setpoint(0);
left_motor.run(false);
}
if(res.vr!=0)
{
right_motor.set_pulses_per_second_setpoint(res.vr);
right_motor.set_time_setpoint(req.time);
right_motor.run(true);
}
else
{
right_motor.set_pulses_per_second_setpoint(0);
right_motor.run(false);
}
//cout << "received " << vx << "," << wt << "=>" << vl <<"," << vr << endl;
}
void odometry()
{
int encoder[2]={0,0}, prev_encoder[2] = {0,0}, dl, dr; // dl is change in left motor and dr is change in right motor
if(new_estimate)
{ // if new_estimate is true
x = x_est;
y = y_est;
t = t_est;
new_estimate = false;
}
encoder[0] = left_motor.position();
encoder[1] = right_motor.position();
dl = encoder[0]-prev_encoder[0];
dr = encoder[1]-prev_encoder[1]; // current position- prev position
x += cos(t)*speed*(dl+dr)/2.0;
y += sin(t)*speed*(dl+dr)/2.0; // avg. of dl and dr are takin i guess
t += speed*(dl-dr)/L; // t is theta
prev_encoder[0] = encoder[0];
prev_encoder[1] = encoder[1];
vl = left_motor.pulses_per_second(); // this function returns the velocity.
vr = right_motor.pulses_per_second();
vx = (vl+vr)/2*speed;
wt = (vl-vr)/L*speed;
}
void run_to_position(motor &m, int sleep_ms) {
m.run_to_abs_pos();
while (m.state().count("holding") == 0) {
this_thread::sleep_for(chrono::milliseconds(100));
}
if (sleep_ms > 0) {
this_thread::sleep_for(chrono::milliseconds(sleep_ms));
}
}
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;
}
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.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');
}
