-
Notifications
You must be signed in to change notification settings - Fork 35
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Software Challenge Urban Pistek #11
base: master
Are you sure you want to change the base?
Changes from 1 commit
f5e0cf9
4b71dcd
aabc1c4
249c968
7a5507a
ba63d7b
17ce177
6fb4cc4
1876046
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -83,25 +83,29 @@ int main(int argc, char **argv) { | |
} | ||
} | ||
|
||
turtleDistance turtleDist(x_coords[0], y_coords[0], x_coords[1], y_coords[1]); | ||
//Publisher for turtle distances | ||
ros::Publisher turtleDist_pub = nh.advertise<turtleDistances>("turtle_distnaces", 10); | ||
turtleDist_pub.publish(turtleDist); | ||
|
||
//Creating a Action Client | ||
actionlib::SimpleActionClient<actionlib> ac("moveTurtle", true); | ||
ROS_INFO("Waiting for Action Server to start"); | ||
ac.waitForServer(); | ||
//Define the waypoint for which the turtle should move to | ||
int x = 0; | ||
int y = 0; | ||
int goal[2] = {x, y}; | ||
ac.sendGoal(goal); | ||
|
||
bool waitForAction = ac.waitForResult(ros::Duration(30.0)); | ||
if(waitForAction){ | ||
ROS_INFO("Action Finished"); | ||
} else { | ||
ROS_INFO("Action Failed"); | ||
//Infinite loop to run | ||
while(true){ | ||
turtleDistance turtleDist(x_coords[0], y_coords[0], x_coords[1], y_coords[1]); | ||
//Publisher for turtle distances | ||
ros::Publisher turtleDist_pub = nh.advertise<turtleDistances>("turtle_distnaces", 10); | ||
turtleDist_pub.publish(turtleDist); | ||
|
||
//Creating a Action Client | ||
actionlib::SimpleActionClient<actionlib> ac("moveTurtle", true); | ||
ROS_INFO("Waiting for Action Server to start"); | ||
ac.waitForServer(); | ||
//Define the waypoint for which the turtle should move to | ||
int x = 0; | ||
int y = 0; | ||
int goal[2] = {x, y}; | ||
ac.sendGoal(goal); | ||
|
||
bool waitForAction = ac.waitForResult(ros::Duration(30.0)); | ||
if(waitForAction){ | ||
ROS_INFO("Action Finished"); | ||
} else { | ||
ROS_INFO("Action Failed"); | ||
} | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. you're missing the ros::spinOnce function, this is the function that checks for new msgs, service calls, actions and other. its what lets us communicate between many applications running ros |
||
} | ||
|
||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -4,43 +4,80 @@ Action server for moving the turtle | |
|
||
#include <ros/ros.h> | ||
#include <actionlib/server/simple_action_server.h> | ||
#include <software_training_assignment/moveTurtle.h> | ||
//Subscribe to the geometry_msgs/Twist topic | ||
#include <geometry_msgs/Twist.h> | ||
#include <math.h> | ||
|
||
bool moveTurtle(int x, int y){ | ||
|
||
//Class to define the action server | ||
class MoveTurtleAction { | ||
protected: | ||
ros::NodeHandle nh; | ||
actionlib::SimpleActionServer<software_training_assignment::moveTurtle> as; | ||
std::string action_name; | ||
software_training_assignment::moveTurtleFeedback feedback; | ||
software_training_assignment::moveTurtleResult result; | ||
|
||
return success; | ||
} | ||
public: | ||
|
||
int main(int argc, char **argv) { | ||
ros::init(argc, argv, "moveTurtle"); | ||
ros::NodeHandle nh; | ||
MoveTurtleAction(std::string name) : | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. copy nodehandle to class nodehandle There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Do you mean to put nodehandle in its own class? |
||
as(nh, name, boost::bind(&MoveTurtleAction::executeCB, this, _1), false, action_name(name)) { | ||
as.start(); | ||
} | ||
|
||
~MoveTurtleAction(void){} | ||
|
||
void executeCB(const software_training_assignment::moveTurtleGoalConstPtr &goal){ | ||
|
||
bool success = false; | ||
geometry_msgs::Twist vel_msg; | ||
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10); | ||
int end_pos_x = goal->abs_x; | ||
int end_pos_y = goal->abs_y; | ||
double distance = sqrt(end_pos_x*end_pos_x + end_pos_y*end_pos_y); | ||
double elapsed_dist = 0; | ||
|
||
//start time | ||
double t0 = ros::Time::now().toSec(); | ||
//move turtle to the end position using the unit vector for the velocity | ||
vel_msg.linear.x = (end_pos_x/distance); | ||
vel_msg.linear.y = (end_pos_y/distance); | ||
vel_msg.linear.z = 0; | ||
vel_msg.angular.x = 0; | ||
vel_msg.angular.y = 0; | ||
vel_msg.angular.z = 0; | ||
|
||
//Create the action | ||
actionlib::SimpleActionServer<actionlib> as; | ||
std::string action_name = "moveTurtle"; | ||
//publisher message | ||
pub.publish(vel_msg); | ||
|
||
while(elapsed_dist < distance){ | ||
double t1 = ros::Time::now().toSec(); | ||
double t_diff = t1 - t0; | ||
|
||
//Create messages to be published to feedback and result | ||
actionlib::Feedback feedback; | ||
actionlib::Result result; | ||
elapsed_dist = sqrt(vel_msg.linear.x*vel_msg.linear.x + vel_msg.linear.y*vel_msg.linear.y)*t_diff; | ||
feedback.abs_distance = elapsed_dist; | ||
} | ||
|
||
//Get the pointer to the action goal | ||
const actionlib::GoalConstPtr &goal; | ||
//Set the end destination for the action | ||
int x = goal->x; | ||
int y = goal->y; | ||
//stop the turtle from moving | ||
vel_msg.linear.x = 0; | ||
vel_msg.linear.y = 0; | ||
pub.publish(vel_msg); | ||
|
||
//Subscribe to the turtleX/cmd_vel topic | ||
ros::Subscriber sub = nh.subscribe("cmd_vel", 10, moveTurtle); | ||
//Access the message type parameters | ||
geometry_msgs::Twist::vector3::x = x; | ||
//Get the stop time | ||
double t_end = ros::Time::now().toSec(); | ||
|
||
//Create the action server | ||
as(nh, action_name); | ||
as.start(); | ||
//set the result | ||
result.time = (int) (t_end - t0); | ||
|
||
ros::spin(); | ||
} | ||
}; | ||
|
||
int main(int argc, char **argv) { | ||
|
||
ros::init(argc, argv, "moveTurtle"); | ||
MoveTurtleAction moveTurtle("moveTurtle"); | ||
ros::spin(); | ||
|
||
return 0; | ||
|
||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
if you look at the tutorials we want all loops to kill if the ros master is killed so we usually use while (ros::ok())