assisted_teleop.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
38 
39 namespace assisted_teleop {
40  AssistedTeleop::AssistedTeleop() : costmap_ros_("costmap", tf_), planning_thread_(NULL){
41  ros::NodeHandle private_nh("~");
42  private_nh.param("controller_frequency", controller_frequency_, 10.0);
43  private_nh.param("num_th_samples", num_th_samples_, 20);
44  private_nh.param("num_x_samples", num_x_samples_, 10);
45  private_nh.param("theta_range", theta_range_, 0.7);
46  private_nh.param("translational_collision_speed", collision_trans_speed_, 0.0);
47  private_nh.param("rotational_collision_speed", collision_rot_speed_, 0.0);
48  planner_.initialize("planner", &tf_, &costmap_ros_);
49 
51  pub_ = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
52  sub_ = n.subscribe("teleop_cmd_vel", 10, &AssistedTeleop::velCB, this);
53  cmd_vel_.linear.x = 0.0;
54  cmd_vel_.linear.y = 0.0;
55  cmd_vel_.linear.z = 0.0;
56 
57  planning_thread_ = new boost::thread(boost::bind(&AssistedTeleop::controlLoop, this));
58  }
59 
61  planning_thread_->join();
62  delete planning_thread_;
63  }
64 
65  void AssistedTeleop::velCB(const geometry_msgs::TwistConstPtr& vel){
66  boost::mutex::scoped_lock lock(mutex_);
67  cmd_vel_ = *vel;
68  }
69 
72  while(ros::ok()){
73  Eigen::Vector3f desired_vel = Eigen::Vector3f::Zero();
74 
75  //we'll copy over odometry and velocity data for planning
76  {
77  boost::mutex::scoped_lock lock(mutex_);
78  desired_vel[0] = cmd_vel_.linear.x;
79  desired_vel[1] = cmd_vel_.linear.y;
80  desired_vel[2] = cmd_vel_.angular.z;
81  }
82 
83  //first, we'll check the trajectory that the user sent in... if its legal... we'll just follow it
84  if(planner_.checkTrajectory(desired_vel[0], desired_vel[1], desired_vel[2], true)){
85  geometry_msgs::Twist cmd;
86  cmd.linear.x = desired_vel[0];
87  cmd.linear.y = desired_vel[1];
88  cmd.angular.z = desired_vel[2];
89  pub_.publish(cmd);
90  r.sleep();
91  continue;
92  }
93 
94  double dth = (theta_range_) / double(num_th_samples_);
95  double dx = desired_vel[0] / double(num_x_samples_);
96  double start_th = desired_vel[2] - theta_range_ / 2.0 ;
97 
98  Eigen::Vector3f best = Eigen::Vector3f::Zero();
99  double best_dist = DBL_MAX;
100  bool trajectory_found = false;
101 
102  //if we don't have a valid trajectory... we'll start checking others in the angular range specified
103  for(int i = 0; i < num_x_samples_; ++i){
104  Eigen::Vector3f check_vel = Eigen::Vector3f::Zero();
105  check_vel[0] = desired_vel[0] - i * dx;
106  check_vel[1] = desired_vel[1];
107  check_vel[2] = start_th;
108  for(int j = 0; j < num_th_samples_; ++j){
109  check_vel[2] = start_th + j * dth;
110  if(planner_.checkTrajectory(check_vel[0], check_vel[1], check_vel[2], false)){
111  //if we have a legal trajectory, we'll score it based on its distance to our desired velocity
112  Eigen::Vector3f diffs = (desired_vel - check_vel);
113  double sq_dist = diffs[0] * diffs[0] + diffs[1] * diffs[1] + diffs[2] * diffs[2];
114 
115  //if we have a trajectory that is better than our best one so far, we'll take it
116  if(sq_dist < best_dist){
117  best = check_vel;
118  best_dist = sq_dist;
119  trajectory_found = true;
120  }
121  }
122  }
123  }
124 
125  //check if best is still zero, if it is... scale the original trajectory based on the collision_speed requested
126  //but we only need to do this if the user has set a non-zero collision speed
127  if(!trajectory_found && (collision_trans_speed_ > 0.0 || collision_rot_speed_ > 0.0)){
128  double trans_scaling_factor = 0.0;
129  double rot_scaling_factor = 0.0;
130  double scaling_factor = 0.0;
131 
132  if(fabs(desired_vel[0]) > 0 && fabs(desired_vel[1]) > 0)
133  trans_scaling_factor = std::min(collision_trans_speed_ / fabs(desired_vel[0]), collision_trans_speed_ / fabs(desired_vel[1]));
134  else if(fabs(desired_vel[0]) > 0)
135  trans_scaling_factor = collision_trans_speed_ / (fabs(desired_vel[0]));
136  else if(fabs(desired_vel[1]) > 0)
137  trans_scaling_factor = collision_trans_speed_ / (fabs(desired_vel[1]));
138 
139  if(fabs(desired_vel[2]) > 0)
140  rot_scaling_factor = collision_rot_speed_ / (fabs(desired_vel[2]));
141 
143  scaling_factor = std::min(trans_scaling_factor, rot_scaling_factor);
144  else if(collision_trans_speed_ > 0.0)
145  scaling_factor = trans_scaling_factor;
146  else if(collision_rot_speed_ > 0.0)
147  scaling_factor = rot_scaling_factor;
148 
149  //apply the scaling factor
150  best = scaling_factor * best;
151  }
152 
153  geometry_msgs::Twist best_cmd;
154  best_cmd.linear.x = best[0];
155  best_cmd.linear.y = best[1];
156  best_cmd.angular.z = best[2];
157  pub_.publish(best_cmd);
158 
159  r.sleep();
160  }
161  }
162 };
163 
164 
165 int main(int argc, char** argv){
166  ros::init(argc, argv, "assisted_teleop");
168  ros::spin();
169  return 0;
170 }
void initialize(std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
string cmd
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf::TransformListener * tf_
ROSCPP_DECL void spin(Spinner &spinner)
#define DBL_MAX
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
base_local_planner::TrajectoryPlannerROS planner_
bool sleep()
bool checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map=true)
costmap_2d::Costmap2DROS costmap_ros_
int main(int argc, char **argv)
void velCB(const geometry_msgs::TwistConstPtr &vel)
geometry_msgs::Twist cmd_vel_


assisted_teleop
Author(s): Tully Foote
autogenerated on Tue Apr 2 2019 02:34:40