1 #ifndef PHEENO_ROS_SIM_PHEENO_ROBOT_H 2 #define PHEENO_ROS_SIM_PHEENO_ROBOT_H 5 #include "std_msgs/Float32.h" 6 #include "std_msgs/Int16.h" 7 #include "geometry_msgs/Twist.h" 8 #include "geometry_msgs/Vector3.h" 9 #include "nav_msgs/Odometry.h" 54 float angular_velocity = 1.2,
float linear_velocity = 0.08,
55 double range_to_avoid = 20.0);
57 float angular_velocity = 1.2,
double range_to_avoid = 20.0);
60 void publish(geometry_msgs::Twist velocity);
100 void odomCallback(
const nav_msgs::Odometry::ConstPtr& msg);
117 #endif //GUARD_PHEENO_ROBOT_H bool irSensorTriggered(float sensor_limits)
void irSensorCLeftCallback(const std_msgs::Float32::ConstPtr &msg)
void accelerometerCallback(const geometry_msgs::Vector3::ConstPtr &msg)
bool checkFrontColor(int color)
void publish(geometry_msgs::Twist velocity)
void irSensorBottomCallback(const std_msgs::Int16::ConstPtr &msg)
void gyroscopeCallback(const geometry_msgs::Vector3::ConstPtr &msg)
ros::Subscriber sub_pheeno_cam_
std::vector< double > gyroscope_values_
PheenoRobot(std::string pheeno_name)
ros::Subscriber sub_ir_cr_
std::vector< double > magnetometer_values_
ros::Subscriber sub_odom_
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
void encoderRLCallback(const std_msgs::Int16::ConstPtr &msg)
void irSensorCenterCallback(const std_msgs::Float32::ConstPtr &msg)
std_msgs::Float32 ir_sensor_back_
std_msgs::Float32 ir_sensor_center_
ros::Subscriber sub_gyroscope_
std::vector< double > odom_pose_position_
ros::Subscriber sub_encoder_RL_
ros::Subscriber sub_magnetometer_
nav_msgs::Odometry odom_msg_
void irSensorBackCallback(const std_msgs::Float32::ConstPtr &msg)
ros::Subscriber sub_ir_left_
ros::Subscriber sub_encoder_LR_
std_msgs::Int16 ir_sensor_bottom_
std_msgs::Float32 ir_sensor_c_right_
std::vector< double > odom_twist_angular_
void magnetometerCallback(const geometry_msgs::Vector3::ConstPtr &msg)
ros::Subscriber sub_ir_bottom_
std::vector< double > odom_pose_orient_
std_msgs::Float32 ir_sensor_c_left_
std_msgs::Float32 ir_sensor_right_
ros::Subscriber sub_encoder_RR_
ros::Subscriber sub_ir_center_
ros::Subscriber sub_ir_right_
std::vector< double > odom_twist_linear_
ros::Subscriber sub_accelerometer_
void encoderLRCallback(const std_msgs::Int16::ConstPtr &msg)
ros::Subscriber sub_encoder_LL_
ros::Publisher pub_cmd_vel_
void avoidObstaclesAngular(double &angular, double &random_turn_value, float angular_velocity=1.2, double range_to_avoid=20.0)
ros::Subscriber sub_ir_back_
ros::Subscriber sub_ir_cl_
void encoderLLCallback(const std_msgs::Int16::ConstPtr &msg)
void encoderRRCallback(const std_msgs::Int16::ConstPtr &msg)
void irSensorLeftCallback(const std_msgs::Float32::ConstPtr &msg)
std::vector< double > ir_sensor_values_
double randomTurn(float angular=0.06)
void irSensorCRightCallback(const std_msgs::Float32::ConstPtr &msg)
std_msgs::Float32 ir_sensor_left_
std::vector< int > encoder_values_
std::vector< double > accelerometer_values_
std::string pheeno_namespace_id_
void irSensorRightCallback(const std_msgs::Float32::ConstPtr &msg)
std::vector< bool > color_state_facing_
void avoidObstaclesLinear(double &linear, double &angular, float angular_velocity=1.2, float linear_velocity=0.08, double range_to_avoid=20.0)