40 "Usage : stdr_obstacle avoidance <robot_frame_id> <laser_frame_id>");
44 std::string(argv[1]) + std::string(
"/") + std::string(argv[2]);
46 std::string(argv[1]) + std::string(
"/cmd_vel");
74 float linear = 0, rotational = 0;
75 for(
unsigned int i = 0 ; i <
scan_.ranges.size() ; i++)
77 float real_dist =
scan_.ranges[i];
78 linear -= cos(
scan_.angle_min + i *
scan_.angle_increment)
79 / (1.0 + real_dist * real_dist);
80 rotational -= sin(
scan_.angle_min + i *
scan_.angle_increment)
81 / (1.0 + real_dist * real_dist);
83 geometry_msgs::Twist cmd;
85 linear /=
scan_.ranges.size();
86 rotational /=
scan_.ranges.size();
94 else if(linear < -0.3)
99 cmd.linear.x = 0.3 + linear;
100 cmd.angular.z = rotational;
ros::NodeHandle n_
The laser topic.
ros::Publisher cmd_vel_pub_
std::string laser_topic_
The speeds topic.
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())
ros::Subscriber subscriber_
The ROS node handle.
sensor_msgs::LaserScan scan_
< The ros laser scan msg
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void callback(const sensor_msgs::LaserScan &msg)
Callback for the ros laser message.
ObstacleAvoidance(int argc, char **argv)
Default contructor.
std::string speeds_topic_
The twist publisher.
~ObstacleAvoidance(void)
Default destructor.
The main namespace for STDR Samples.