bool check_action(billiards_msgs::TableState ts,double angle_min,double angle_max)
{
actionlib::SimpleActionClient<billiards_msgs::PlanShotAction> action_client("plan_shot", true);
action_client.waitForServer();
ROS_INFO("plan_shot action server is up, sending goal");
billiards_msgs::PlanShotGoal goal;
goal.state = ts;
goal.angle_min = angle_min;
goal.angle_max = angle_max;
action_client.sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = action_client.waitForResult(ros::Duration(30.0));
if (finished_before_timeout)
{
actionlib::SimpleClientGoalState state = action_client.getState();
ROS_INFO("Action finished: %s",state.toString().c_str());
return (state == actionlib::SimpleClientGoalState::SUCCEEDED);
}
else
{
ROS_INFO("Action did not finish before the time out.");
return false;
}
}
void InterestingMoves::find_and_send_trajectory(Vectorq7x1 position)
{
Vectorq7x1 q_pose;
q_pose << position;
Eigen::VectorXd q_in_vecxd;
Vectorq7x1 q_vec_right_arm;
std::vector<Eigen::VectorXd> des_path;
trajectory_msgs::JointTrajectory des_trajectory;
Baxter_traj_streamer baxter_traj_streamer(&nh_);
ROS_INFO("warming up callbacks...");
for (int i = 0; i < 100; i++)
{
ros::spinOnce();
ros::Duration(0.01).sleep();
}
// find the current pose of the robot
ROS_INFO("getting current right arm pose");
q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ROS_INFO_STREAM("r_arm state: " << q_vec_right_arm.transpose());
// push back the current pose of the robot, followed by the desired pose for the robot
q_in_vecxd = q_vec_right_arm;
des_path.push_back(q_in_vecxd);
q_in_vecxd = q_pose;
des_path.push_back(q_in_vecxd);
// convert this into a trajectory
ROS_INFO("stuffing trajectory");
baxter_traj_streamer.stuff_trajectory(des_path, des_trajectory);
// copy this trajectory into a goal message
baxter_traj_streamer::trajGoal goal;
goal.trajectory = des_trajectory;
// initialize this node as an action client
actionlib::SimpleActionClient<baxter_traj_streamer::trajAction> action_client("trajActionServer", true);
ROS_INFO("waiting for server: ");
bool server_exists = action_client.waitForServer(ros::Duration(5.0));
if (!server_exists)
{
ROS_WARN("could not connect to server");
return;
}
// server_exists = action_client.waitForServer(); // wait forever
ROS_INFO("connected to action server"); // if here, then we connected to the server;
g_count++;
goal.traj_id = g_count;
ROS_INFO("sending traj_id %d", g_count);
// send the goal to the server
action_client.sendGoal(goal);
bool finished_before_timeout = action_client.waitForResult(); // wait forever for result
}
void close_fingers(){
actionlib::SimpleActionClient<jaco_msgs::SetFingersPositionAction> action_client("jaco/finger_joint_angles",true);
action_client.waitForServer();
jaco_msgs::SetFingersPositionGoal fingers = jaco_msgs::SetFingersPositionGoal();
fingers.fingers.Finger_1 = 60;
fingers.fingers.Finger_2 = 60;
fingers.fingers.Finger_3 = 60;
action_client.sendGoal(fingers);
wait_for_fingers_stopped();
std::cout << "Grasp completed" << std::endl;
}
void move_up(double distance){
actionlib::SimpleActionClient<jaco_msgs::ArmPoseAction> action_client("/jaco/arm_pose",true);
action_client.waitForServer();
jaco_msgs::ArmPoseGoal pose_goal = jaco_msgs::ArmPoseGoal();
arm_mutex.lock();
pose_goal.pose = arm_pose;
pose_goal.pose.header.frame_id = "/jaco_api_origin";
pose_goal.pose.pose.position.z += distance;
arm_mutex.unlock();
action_client.sendGoal(pose_goal);
wait_for_arm_stopped();
}
/**
* Callback for the "command" topic. There are two ways of sending a trajectory:
* 1. by publishing a message to the "command" topic (this is what happens here)
* 2. by using the action server (see executeCB)
*/
void JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
{
ROS_WARN("commandCB() called, this is not tested yet");
// just creates an action from the message and sends it on to the action server
// create an action client spinning its own thread
JTAC action_client("katana_arm_controller/joint_trajectory_action", true);
action_client.waitForServer();
JTAS::Goal goal;
goal.trajectory = *(msg.get());
// fire and forget
action_client.sendGoal(goal);
}
int main(int argc, char** argv) {
ros::init(argc, argv, "traj_action_client_node"); // name this node
int g_count = 0;
// here is a "goal" object compatible with the server, as defined in example_action_server/action
my_interesting_moves::trajGoal goal;
// use the name of our server, which is: example_action (named in example_action_server.cpp)
actionlib::SimpleActionClient<my_interesting_moves::trajAction> action_client("trajActionServer", true);
// attempt to connect to the server:
ROS_INFO("waiting for server: ");
bool server_exists = action_client.waitForServer(ros::Duration(5.0)); // wait for up to 5 seconds
// something odd in above: does not seem to wait for 5 seconds, but returns rapidly if server not running
if (!server_exists) {
ROS_WARN("could not connect to server; will wait forever");
return 0; // bail out; optionally, could print a warning message and retry
}
server_exists = action_client.waitForServer(); //wait forever
ROS_INFO("connected to action server"); // if here, then we connected to the server;
while(true) {
// stuff a goal message:
g_count++;
goal.traj_id = g_count; // this merely sequentially numbers the goals sent
ROS_INFO("sending traj_id %d",g_count);
//action_client.sendGoal(goal); // simple example--send goal, but do not specify callbacks
action_client.sendGoal(goal,&doneCb); // we could also name additional callback functions here, if desired
// action_client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb); //e.g., like this
bool finished_before_timeout = action_client.waitForResult(ros::Duration(5.0));
//bool finished_before_timeout = action_client.waitForResult(); // wait forever...
if (!finished_before_timeout) {
ROS_WARN("giving up waiting on result for goal number %d",g_count);
return 0;
}
else {
ROS_INFO("finished before timeout");
}
}
return 0;
}
void move_to_grasp_point(double x, double y, double z, double rotx, double roty, double rotz, double rotw){
actionlib::SimpleActionClient<jaco_msgs::ArmPoseAction> action_client("/jaco/arm_pose",true);
action_client.waitForServer();
jaco_msgs::ArmPoseGoal pose_goal = jaco_msgs::ArmPoseGoal();
pose_goal.pose.header.frame_id = "/jaco_api_origin";
pose_goal.pose.pose.position.x = x;
pose_goal.pose.pose.position.y = y;
pose_goal.pose.pose.position.z = z;
pose_goal.pose.pose.orientation.x = rotx;
pose_goal.pose.pose.orientation.y = roty;
pose_goal.pose.pose.orientation.z = rotz;
pose_goal.pose.pose.orientation.w = rotw;
action_client.sendGoal(pose_goal);
wait_for_arm_stopped();
}
int main(int argc, char **argv) {
ros::init(argc, argv, "two_wheel_dispersion");
ros::NodeHandle nh;
// get initialization message of robot swarm from parameter server
std::string robot_model_name;
int robot_quantity;
bool get_name, get_quantity;
get_name = nh.getParam("/robot_model_name", robot_model_name);
get_quantity = nh.getParam("/robot_quantity", robot_quantity);
if (!(get_name && get_quantity))
return 0; // return if fail to get parameter
// get settings for this simulation from private parameter
// parameter: spring_length
bool get_spring_length = nh.getParam("spring_length", spring_length);
if (get_spring_length)
ROS_INFO_STREAM("using spring_length passed in: " << spring_length);
else
ROS_INFO_STREAM("using default spring_length: " << spring_length);
// parameter: wheel_speed
bool get_wheel_speed = nh.getParam("wheel_speed", wheel_speed);
if (get_wheel_speed)
ROS_INFO_STREAM("using wheel_speed passed in: " << wheel_speed);
else
ROS_INFO_STREAM("using default wheel_speed: " << wheel_speed);
// initialize a subscriber to topic "swarm_robot_poses"
ros::Subscriber swarm_robot_poses_subscriber = nh.subscribe("swarm_robot_poses", 1, swarmRobotPosesCb);
// make sure topics "swarm_robot_poses" are active
while (!g_robot_poses_cb_started) {
ros::Duration(0.5).sleep();
ros::spinOnce();
}
std::cout << "topic message from swarm_robot_poses is ready" << std::endl;
// initialize an action client
actionlib::SimpleActionClient<swarm_robot_action::swarm_robot_trajAction> action_client(
"two_wheel_traj_action", true);
swarm_robot_action::swarm_robot_trajGoal goal; // instantiate a goal message
// try to connect the client to action server
bool server_exist = action_client.waitForServer(ros::Duration(5.0));
ros::Duration(1.0).sleep();
while (!server_exist) {
ROS_WARN("could not connect to server; retrying");
bool server_exist = action_client.waitForServer(ros::Duration(1.0));
ros::Duration(1.0).sleep();
}
// if here, then connected to the server
ROS_INFO("connected to action server");
// variables for the loop
double distance[robot_quantity][robot_quantity]; // two dimensional array to store distance
double distance_sort[robot_quantity][robot_quantity];
int index_sort[robot_quantity][robot_quantity];
// the loop of optimizing robot position for dispersion
int iteration_index = 0;
while (ros::ok()) {
iteration_index = iteration_index + 1;
ROS_INFO_STREAM(""); // blank line
ROS_INFO_STREAM("iteration index: " << iteration_index); // iteration index
ros::spinOnce(); // let robot positions update, will be used later
// calculate the distance between robots
for (int i=0; i<robot_quantity; i++) {
for (int j=i; j<robot_quantity; j++) {
distance[i][j] = sqrt(pow(g_robot_x[i] - g_robot_x[j], 2) +
pow(g_robot_y[i] - g_robot_y[j], 2));
if (i != j) // not in diagonal axis
distance[j][i] = distance[i][j]; // symmetrical matrix
}
}
// initialize distance_sort and index_sort
for (int i=0; i<robot_quantity; i++)
for (int j=0; j<robot_quantity; j++) {
distance_sort[i][j] = distance[i][j];
index_sort[i][j] = j;
}
// sort the distances and get sorted index
double distance_temp;
int index_temp;
for (int i=0; i<robot_quantity; i++) {
for (int j=0; j<robot_quantity-1; j++) {
for (int k=0; k<robot_quantity-1-j; k++) {
if (distance_sort[i][k] > distance_sort[i][k+1]) {
// switch value between distance_sort[i][k] and distance_sort[i][k+1]
distance_temp = distance_sort[i][k];
distance_sort[i][k] = distance_sort[i][k+1];
distance_sort[i][k+1] = distance_temp;
// switch value between index_sort[i][k] and index[i][k+1]
index_temp = index_sort[i][k];
index_sort[i][k] = index_sort[i][k+1];
index_sort[i][k+1] = index_temp;
}
}
}
}
// check whether the closest is itself
for (int i=0; i<robot_quantity; i++) {
if (index_sort[i][0] != i) {
ROS_WARN("error in the sorting of robot distance");
return 0;
}
}
// find all the neighbors that will be used in the force feedback control
// the algorithm used here is the same with the algorithm implemented in matlab before
// no matter how far away one robot is with others, 3 closest neighbors will be counted
// no matter how close one robot is with others, 6 closest neighbors at most will be counted
// which means there should be at least 6 robots
int neighbor_num[robot_quantity]; // the number of valid neighbors
for (int i=0; i<robot_quantity; i++) {
neighbor_num[i] = 3; // there are at least 3 neighbors
while (true) {
neighbor_num[i] = neighbor_num[i] + 1;
if (neighbor_num[i] <= 6)
if (distance_sort[i][neighbor_num[i]] < upper_limit)
continue;
else {
neighbor_num[i] = neighbor_num[i] - 1;
break;
}
else {
neighbor_num[i] = neighbor_num[i] - 1;
break;
}
}
// ROS_INFO_STREAM("neighbor number of " << i << ": " << neighbor_num[i]);
}
// for (int i=0; i<robot_quantity; i++) {
// neighbor_num[i] = 3; // there are at least 3 neighbors
// while (distance_sort[i][neighbor_num[i]+1] < upper_limit && neighbor_num[i] <= 6)
// neighbor_num[i] = neighbor_num[i] + 1;
// }
// calculate displacement of each robot
double displacement_x[robot_quantity]; // displacement in x
double displacement_y[robot_quantity]; // displacement in y
double distance_diff;
for (int i=0; i<robot_quantity; i++) {
displacement_x[i] = 0.0;
displacement_y[i] = 0.0;
double distance_diff_sum = 0.0;
for (int j=1; j<=neighbor_num[i]; j++) {
distance_diff = distance_sort[i][j] - spring_length;
// add the spring effect of all the neighbors
displacement_x[i] = displacement_x[i] + feedback_ratio * distance_diff *
(g_robot_x[index_sort[i][j]] - g_robot_x[i]) / distance_sort[i][j];
displacement_y[i] = displacement_y[i] + feedback_ratio * distance_diff *
(g_robot_y[index_sort[i][j]] - g_robot_y[i]) / distance_sort[i][j];
}
}
// prepare the goal message
double displacement_average = 0.0; // for the calculation of time cost
goal.x.resize(robot_quantity); // important here, otherwise runtime error
goal.y.resize(robot_quantity); // important here, otherwise runtime error
for (int i=0; i<robot_quantity; i++) {
goal.x[i] = g_robot_x[i] + displacement_x[i];
goal.y[i] = g_robot_y[i] + displacement_y[i];
displacement_average = displacement_average +
sqrt(pow(displacement_x[i], 2) + pow(displacement_y[i], 2));
}
// this is the real average displacement
displacement_average = displacement_average / robot_quantity;
// use displacement_average to represent time spent on line movement
// use M_PI/3 to represent time spent on self rotating
goal.time_cost = displacement_average / wheel_radius / wheel_speed +
M_PI/3 * half_wheel_dist / wheel_radius / wheel_speed;
ROS_INFO_STREAM("time cost for action: " << goal.time_cost << "(second)");
// send out goal
action_client.sendGoal(goal);
// wait for expected duration plus some tolerance (2 seconds)
bool finish_before_timeout = action_client.waitForResult(ros::Duration(goal.time_cost + 2.0));
if (!finish_before_timeout) {
ROS_WARN("this action is not done...timeout");
return 0;
}
else {
ROS_INFO("this action is done.");
}
}
return 0;
}
void DynamicMovementPrimitiveGUI::learn()
{
std::vector<task_recorder2_msgs::Description> robot_part_descriptions;
// if(!widget_list_map_.at(robot_part_list_).getDescriptions(robot_part_descriptions))
getSelectedDescriptionsSignal(robot_part_list_, robot_part_descriptions);
if(robot_part_descriptions.empty())
{
setStatusReport("No robot part selected...", ERROR);
return;
}
actionlib::SimpleActionClient<dmp_behavior_actions::LearningFromDemonstrationAction> action_client("/Behaviors/learningFromDemonstration", true);
bool server_online = false;
while(!server_online)
{
if(action_client.waitForServer(ros::Duration(0.5)))
{
server_online = true;
}
else
{
ROS_WARN("Waiting for action server to come online.");
}
ros::spinOnce();
}
std::vector<std::string> robot_part_names;
for (int i = 0; i < (int)robot_part_descriptions.size(); ++i)
{
robot_part_names.push_back(robot_part_descriptions[i].description);
}
std::vector<task_recorder2_msgs::Description> trajectory_descriptions;
// if(!widget_list_map_.at(trajectory_list_).getDescriptions(trajectory_descriptions))
getSelectedDescriptionsSignal(trajectory_list_, trajectory_descriptions);
if(trajectory_descriptions.empty())
{
setStatusReport("No trajectory selected...", ERROR);
return;
}
for (int i = 0; i < (int)trajectory_descriptions.size(); ++i)
{
dmp_behavior_actions::LearningFromDemonstrationGoal goal;
std::string dmp_name = task_recorder2_utilities::getFileName(trajectory_descriptions[i]);
std::string bag_file_name = dmp_name;
task_recorder2_utilities::appendBagFileAppendix(bag_file_name);
goal.joint_states_bag_file_name = bag_file_name;
goal.robot_part_names_from_trajectory = robot_part_names;
goal.object.name = dmp_name;
dynamic_movement_primitive::TypeMsg type;
if(task_radio_button_->isChecked())
{
type.type = dynamic_movement_primitive::TypeMsg::DISCRETE_CARTESIAN_AND_JOINT_SPACE;
}
else
{
type.type = dynamic_movement_primitive::TypeMsg::DISCRETE_JOINT_SPACE;
}
goal.type = type;
action_client.sendGoal(goal);
bool got_result = false;
while(!got_result)
{
if(action_client.waitForResult(ros::Duration(0.5)))
{
got_result = true;
}
else
{
setStatusReport("Waiting for result...", WARN);
}
ros::spinOnce();
}
if(action_client.getResult()->result == dmp_behavior_actions::LearningFromDemonstrationResult::SUCCEEDED)
{
setStatusReport("Learned DMP " + dmp_name + ".", INFO);
}
else
{
setStatusReport("Problems while learning DMP " + dmp_name + ".", ERROR);
}
load(dmp_data_directory_name_, dmp_list_);
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "action_client_node"); // name this node
ros::NodeHandle n;
ros::Subscriber alarm_sub = n.subscribe("/lidar_alarm", 1, alarmCallback);
actionlib::SimpleActionClient<gazebo_action_motion_ctrl::pathAction> action_client("my_action", true);
gazebo_action_motion_ctrl::pathGoal goal;
ROS_INFO("waiting for server: ");
bool server_exists = action_client.waitForServer(); // wait for up to 5 seconds
// something odd in above: does not seem to wait for 5 seconds, but returns rapidly if server not running
//bool server_exists = action_client.waitForServer(); //wait forever
if (!server_exists) {
ROS_WARN("could not connect to server; halting");
return 0; // bail out; optionally, could print a warning message and retry
}
ROS_INFO("connected to action server"); // if here, then we connected to the server;
geometry_msgs::PoseStamped pose_stamped;
geometry_msgs::Pose pose;
double curr_yaw=0.0;
pose.position.x = 1.0; // say desired x-coord is 1
pose.position.y = 0.0;
pose.position.z = 0.0; // let's hope so!
pose.orientation.x = 0.0; //always, for motion in horizontal plane
pose.orientation.y = 0.0; // ditto
pose.orientation.z = 0.0; // implies oriented at yaw=0, i.e. along x axis
pose.orientation.w = 1.0; //sum of squares of all components of unit quaternion is 1
pose_stamped.pose = pose;
goal.nav_path.poses.push_back(pose_stamped);
action_client.sendGoal(goal, &doneCb);
pose.position.x = 2.0;
pose_stamped.pose = pose;
goal.nav_path.poses.push_back(pose_stamped);
while(true) {
if(g_lidar_alarm){
action_client.cancelAllGoals();
ROS_INFO("Goals Cancelled");
pose=g_curr_pose;
pose_stamped.pose = pose;
goal.nav_path.poses.push_back(pose_stamped);
action_client.sendGoal(goal, &doneCb);
action_client.waitForResult();
if(changedir=="y"){
was_blocked_y=true;
} else if(changedir=="-y"){
ROS_WARN("No Path Found, Aborting");
return 0;
}
was_canceled=true;
}
if(g_is_done){ //checks if the current goal is done, so that it can wait and check lidar alarm
if(was_canceled){
if(changedir=="x"&&!was_blocked_y){
pose.position.y+=1;
changedir="y";
g_strt_dist+=1;
}else {
pose.position.y=pose.position.y-g_strt_dist;
g_strt_dist=1;
changedir="-y";
}
}else{
pose.position.x+=1;
changedir="x";
}
pose_stamped.pose = pose;
goal.nav_path.poses.push_back(pose_stamped);
ROS_INFO("Order up: x = %f y = %f", pose.position.x, pose.position.y);
action_client.sendGoal(goal, &doneCb,&activeCb, &feedbackCb );
g_is_done=false;
}
ros::spinOnce();
goal.nav_path.poses.clear();
}
return 0;
}
