static int cmd_w(char *args){
if(args == NULL){
printf("Please input an expression!\n");
return 0;
}
new_wp(args);
return 0;
}
static int cmd_w(char *args)
{
if(args==NULL)
{
printf("Lack of subcommand \n");
return 0;
}
new_wp(args);
return 0;
}
static int cmd_w( char * args ){
bool if_success;
WP * nwp = new_wp();
if( strlen( args ) > 32 ){
printf("String Len Overflow!\n");
return 0;
}
strcpy( nwp->args, args );
nwp->old_val = expr( args , &if_success);
return 0;
}
static void
SetAirfieldDetails(Waypoints &way_points, const TCHAR *name,
const tstring &Details)
{
const Waypoint *wp = find_waypoint(way_points, name);
if (wp == NULL)
return;
Waypoint new_wp(*wp);
new_wp.details = Details.c_str();
way_points.Replace(*wp, new_wp);
way_points.Optimise();
}
static int cmd_w(char *args)
{
WP * tmp = new_wp();
if(!tmp)
{
printf("Error: can not produce a watchpoint\n");
return 0;
}
strcpy(tmp->expr, args);
bool success;
tmp->newValue = expr(args, &success);
tmp->oldValue = 0;
return 0;
}
static int cmd_w(char *args) {
int i = 0;
bool *flag = malloc(sizeof(bool));
WP *wp = new_wp();
if (args[0] != '*' && args[0] != '$') {
printf("Wrong input!\n");
return 0;
}
printf("Watchpoint No.%d: expr:%s\n", wp->NO, args);
while (args[i] != 0) {
wp->addr[i] = args[i];
++i;
}
wp->old_value = expr(wp->addr, flag);
return 0;
}
static int cmd_w(char *args)
{
if (args==NULL){
printf("Arguement required.(expression to compute)\n");
return 0;
}
/*init_wp_list();
bool success=true;
uint32_t output=expr(args,&success);
if (success)
new_wp(args,output);
else
printf("please try again:\n");
*/
new_wp(args);
return 0;
}
static void
SetAirfieldDetails(Waypoints &way_points, const TCHAR *name,
const tstring &Details,
const std::vector<tstring> &files_external,
const std::vector<tstring> &files_embed)
{
const Waypoint *wp = FindWaypoint(way_points, name);
if (wp == NULL)
return;
Waypoint new_wp(*wp);
new_wp.details = Details.c_str();
new_wp.files_embed.assign(files_embed.begin(), files_embed.end());
#ifdef ANDROID
new_wp.files_external.assign(files_external.begin(), files_external.end());
#endif
way_points.Replace(*wp, new_wp);
way_points.Optimise();
}
static int cmd_watch_exp(char *args) {
bool bb; uint32_t value;
WP* tempWP;
if (args==NULL)
{
printf("error info format [info r/info w]\n");
return 1;
}
value=expr(args,&bb);
if (bb==true)
{
tempWP=new_wp();
strcpy(tempWP->expr,args); //record expression
tempWP->value=value;
}
return 0;
}
/// accepts new goal waypoint and sends the helicopter there
void wpExecuteCB(const asctec_hl_comm::WaypointGoalConstPtr & goal)
{
const geometry_msgs::Point32 & wp = goal->goal_pos;
const float & yaw = goal->goal_yaw;
ROS_INFO("got new waypoint: x=%f y=%f z=%f yaw=%f v_xy=%f v_z=%f accuracy=%f timeout=%f", wp.x, wp.y, wp.z, goal->goal_yaw, goal->max_speed.x, goal->max_speed.z, goal->accuracy_position, goal->timeout);
asctec_hl_comm::mav_ctrlPtr new_wp(new asctec_hl_comm::mav_ctrl);
new_wp->x = wp.x;
new_wp->y = wp.y;
new_wp->z = wp.z;
new_wp->yaw = yaw;
new_wp->v_max_xy = goal->max_speed.x;
new_wp->v_max_z = goal->max_speed.z;
new_wp->type = asctec_hl_comm::mav_ctrl::position;
wp_pub_.publish(new_wp);
std::stringstream feedback_msg;
ros::Rate r(2);
bool success = true;
static asctec_hl_comm::WaypointResult result;
static asctec_hl_comm::WaypointFeedback feedback;
float dist_to_wp = 1e9;
float dist_yaw = M_PI;
float dx, dy, dz;
int i = 0;
int max_cycles = ceil(goal->timeout / r.expectedCycleTime().toSec());
while (ros::ok() && i < max_cycles)
{
if (wp_server_->isPreemptRequested() || !ros::ok())
{
ROS_INFO("waypoint server: Preempted");
// set the action state to preempted
wp_server_->setPreempted();
success = false;
break;
}
boost::mutex::scoped_lock lock(current_pose_mutex_);
dx = wp.x - current_pose_.pose.position.x;
dy = wp.y - current_pose_.pose.position.y;
dz = wp.z - current_pose_.pose.position.z;
dist_to_wp = sqrt(dx * dx + dy * dy + dz * dz);
dist_yaw = fabs(yaw - tf::getYaw(current_pose_.pose.orientation));
dist_yaw = dist_yaw > M_PI ? fabs(dist_yaw - M_PI) : dist_yaw;
feedback_msg.clear();
feedback_msg.str("");
feedback_msg << "dist: " << dist_to_wp << " dist_yaw" << dist_yaw;
if (dist_to_wp <= goal->accuracy_position && dist_yaw <= goal->accuracy_orientation)
break;
feedback.current_pos.x = current_pose_.pose.position.x;
feedback.current_pos.y = current_pose_.pose.position.y;
feedback.current_pos.z = current_pose_.pose.position.z;
feedback.current_yaw = tf::getYaw(current_pose_.pose.orientation);
feedback.status = feedback_msg.str();
lock.unlock();
wp_server_->publishFeedback(feedback);
r.sleep();
i++;
}
result.result_pos.x = current_pose_.pose.position.x;
result.result_pos.y = current_pose_.pose.position.y;
result.result_pos.z = current_pose_.pose.position.z;
result.result_yaw = tf::getYaw(current_pose_.pose.orientation);
if (i == max_cycles)
{
success = false;
wp_server_->setAborted(result, "Timed Out!");
}
if (success)
{
result.status = "Waypoint reached!";
wp_server_->setSucceeded(result);
}
}
static int cmd_w(char*args){
char*arg = strtok(NULL," ");
//printf("%s\n",arg);
new_wp(arg);
return 0;
}
