From 498dfcd857d1c5dd010fc75229553abec5e92aa8 Mon Sep 17 00:00:00 2001 From: "Hunter L. Allen" Date: Sat, 12 Aug 2017 12:21:39 -0700 Subject: [PATCH 1/2] Ported p2os_msgs --- p2os_msgs/AMENT_IGNORE | 0 p2os_msgs/CMakeLists.txt | 53 +++++++++++++++++++++------------- p2os_msgs/msg/BatteryState.msg | 2 +- p2os_msgs/msg/SonarArray.msg | 2 +- p2os_msgs/package.xml | 10 ++++--- 5 files changed, 41 insertions(+), 26 deletions(-) delete mode 100644 p2os_msgs/AMENT_IGNORE diff --git a/p2os_msgs/AMENT_IGNORE b/p2os_msgs/AMENT_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/p2os_msgs/CMakeLists.txt b/p2os_msgs/CMakeLists.txt index d091696..503e071 100644 --- a/p2os_msgs/CMakeLists.txt +++ b/p2os_msgs/CMakeLists.txt @@ -1,27 +1,40 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(p2os_msgs) -find_package(catkin REQUIRED COMPONENTS - message_generation - std_msgs +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +set(msg_files + "msg/TCMState.msg" + "msg/AIO.msg" + "msg/ArmState.msg" + "msg/BatteryState.msg" + "msg/DIO.msg" + "msg/GripperState.msg" + "msg/GripState.msg" + "msg/LiftState.msg" + "msg/MotorState.msg" + "msg/PTZState.msg" + "msg/SonarArray.msg" ) -add_message_files(DIRECTORY msg FILES - TCMState.msg - AIO.msg - ArmState.msg - BatteryState.msg - DIO.msg - GripperState.msg - GripState.msg - LiftState.msg - MotorState.msg - PTZState.msg - SonarArray.msg +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES builtin_interfaces std_msgs ) -generate_messages(DEPENDENCIES std_msgs) +ament_export_dependencies(rosidl_default_runtime) -catkin_package( - CATKIN_DEPENDS message_runtime std_msgs -) \ No newline at end of file +ament_package() diff --git a/p2os_msgs/msg/BatteryState.msg b/p2os_msgs/msg/BatteryState.msg index 2afd78c..0154f81 100644 --- a/p2os_msgs/msg/BatteryState.msg +++ b/p2os_msgs/msg/BatteryState.msg @@ -1,2 +1,2 @@ -Header header +std_msgs/Header header float32 voltage diff --git a/p2os_msgs/msg/SonarArray.msg b/p2os_msgs/msg/SonarArray.msg index fee7d46..2fde31b 100644 --- a/p2os_msgs/msg/SonarArray.msg +++ b/p2os_msgs/msg/SonarArray.msg @@ -1,3 +1,3 @@ -Header header +std_msgs/Header header int32 ranges_count float64[] ranges diff --git a/p2os_msgs/package.xml b/p2os_msgs/package.xml index 80bcc0a..8d7c8c6 100644 --- a/p2os_msgs/package.xml +++ b/p2os_msgs/package.xml @@ -28,10 +28,12 @@ ActivMedia Robotics LLC MobileRobots Inc. - catkin + ament_cmake + rosidl_default_generator - message_generation - std_msgs + std_msgs - message_runtime + + ament_cmake + From 452f7b90f339f3e8cb5700a92de2b1b6194c3e2b Mon Sep 17 00:00:00 2001 From: "Hunter L. Allen" Date: Sat, 12 Aug 2017 13:35:30 -0700 Subject: [PATCH 2/2] Begin porting P2OS driver. Don't try to compile this yet. --- p2os_driver/AMENT_IGNORE | 0 p2os_driver/CMakeLists.txt | 57 +- p2os_driver/include/p2os_driver/p2os.h | 324 +++-- p2os_driver/include/p2os_driver/p2os_ptz.h | 12 +- p2os_driver/include/p2os_driver/packet.h | 8 +- p2os_driver/package.xml | 20 +- p2os_driver/src/p2os.cc | 1464 ++++++++++---------- p2os_driver/src/p2osnode.cc | 9 +- p2os_driver/src/packet.cc | 1 - 9 files changed, 969 insertions(+), 926 deletions(-) delete mode 100644 p2os_driver/AMENT_IGNORE diff --git a/p2os_driver/AMENT_IGNORE b/p2os_driver/AMENT_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/p2os_driver/CMakeLists.txt b/p2os_driver/CMakeLists.txt index d1bb168..fb1b58c 100644 --- a/p2os_driver/CMakeLists.txt +++ b/p2os_driver/CMakeLists.txt @@ -1,30 +1,36 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(p2os_driver) -find_package(catkin REQUIRED COMPONENTS - p2os_msgs - diagnostic_updater - nav_msgs - roscpp - geometry_msgs - tf - std_msgs - kdl_parser -) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() -catkin_package( - INCLUDE_DIRS include - LIBRARIES p2os_driver - CATKIN_DEPENDS p2os_msgs nav_msgs roscpp geometry_msgs diagnostic_updater tf std_msgs kdl_parser -) +find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake_ros REQUIRED) -## Specify additional locations of header files -include_directories(include - ${catkin_INCLUDE_DIRS} +set(req_deps + "p2os_msgs" + "std_msgs" + "nav_msgs" + "rclcpp" + "rcutils" + "geometry_msgs" + "tf2" + "tf2_ros" + "tf2_msgs" + "tf2_geometry_msgs" ) +ament_auto_find_build_dependencies(REQUIRED ${req_deps}) + +INCLUDE_DIRECTORIES(include) + ## Declare a cpp executable -add_executable(p2os_driver +ament_auto_add_executable(p2os_driver src/p2osnode.cc src/p2os.cc src/kinecalc.cc @@ -33,11 +39,12 @@ add_executable(p2os_driver src/sip.cc src/p2os_ptz.cpp ) - -target_link_libraries(p2os_driver ${catkin_LIBRARIES}) -add_dependencies(p2os_driver p2os_msgs_gencpp) +ament_target_dependencies(p2os_driver ${req_deps}) ## Mark executables and/or libraries for installation -install(TARGETS p2os_driver - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + TARGETS p2os_driver + DESTINATION lib/${PROJECT_NAME} ) + +ament_auto_package() diff --git a/p2os_driver/include/p2os_driver/p2os.h b/p2os_driver/include/p2os_driver/p2os.h index ceac9d7..a76dcad 100644 --- a/p2os_driver/include/p2os_driver/p2os.h +++ b/p2os_driver/include/p2os_driver/p2os.h @@ -3,7 +3,8 @@ * Copyright (C) 2009 * David Feil-Seifer, Brian Gerkey, Kasper Stoy, * Richard Vaughan, & Andrew Howard - * + * Copyright (C) 2017 + * Open Source Robotics Foundation, inc. * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -27,49 +28,63 @@ #include #include #include -#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include + +#include + +#include -#include -#include +/* tf2_ros */ +#include +#include +#include +#include + +/* hacky, temporary macros */ +#define ROS_ERROR RCUTILS_LOG_ERROR +#define ROS_INFO RCUTILS_LOG_INFO +#define ROS_DEBUG RCUTILS_LOG_DEBUG + +/* #include */ +/* #include */ /*! \brief Container struct. * * Create a struct that holds the * Robot's sensors. */ -typedef struct ros_p2os_data -{ - //! Provides the position of the robot - nav_msgs::Odometry position; - //! Provides the battery voltage - p2os_msgs::BatteryState batt; - //! Provides the state of the motors (enabled or disabled) - p2os_msgs::MotorState motors; - //! Provides the state of the gripper - p2os_msgs::GripperState gripper; - //! Container for sonar data - p2os_msgs::SonarArray sonar; - //! Digital In/Out - p2os_msgs::DIO dio; - //! Analog In/Out - p2os_msgs::AIO aio; - //! Transformed odometry frame. - geometry_msgs::TransformStamped odom_trans; -} ros_p2os_data_t; +struct ros_p2os_data_t { + //! Provides the position of the robot + nav_msgs::msg::Odometry position; + //! Provides the battery voltage + p2os_msgs::msg::BatteryState batt; + //! Provides the state of the motors (enabled or disabled) + p2os_msgs::msg::MotorState motors; + //! Provides the state of the gripper + p2os_msgs::msg::GripperState gripper; + //! Container for sonar data + p2os_msgs::msg::SonarArray sonar; + //! Digital In/Out + p2os_msgs::msg::DIO dio; + //! Analog In/Out + p2os_msgs::msg::AIO aio; + //! Transformed odometry frame. + geometry_msgs::msg::TransformStamped odom_trans; +} // this is here because we need the above typedef's before including it. #include "sip.h" @@ -85,123 +100,132 @@ class SIP; class P2OSNode { - /*! \brief P2OS robot driver node. - * - * This class contains, essentially, the main means of communication - * between the robot and ROS. - */ - public: - P2OSNode( ros::NodeHandle n ); - virtual ~P2OSNode(); - - public: - //! Setup the robot for use. Communicates with the robot directly. - int Setup(); - //! Prepare for shutdown. - int Shutdown(); - - int SendReceive(P2OSPacket* pkt, bool publish_data = true ); - - void updateDiagnostics(); - - void ResetRawPositions(); - void ToggleSonarPower(unsigned char val); - void ToggleMotorPower(unsigned char val); - void StandardSIPPutData(ros::Time ts); - - inline double TicksToDegrees (int joint, unsigned char ticks); - inline unsigned char DegreesToTicks (int joint, double degrees); - inline double TicksToRadians (int joint, unsigned char ticks); - inline unsigned char RadiansToTicks (int joint, double rads); - inline double RadsPerSectoSecsPerTick (int joint, double speed); - inline double SecsPerTicktoRadsPerSec (int joint, double secs); - - void SendPulse (void); - //void spin(); - void check_and_set_vel(); - void cmdvel_cb( const geometry_msgs::TwistConstPtr &); - - void check_and_set_motor_state(); - void cmdmotor_state( const p2os_msgs::MotorStateConstPtr &); - - void check_and_set_gripper_state(); - void gripperCallback(const p2os_msgs::GripperStateConstPtr &msg); - double get_pulse() {return pulse;} - - // diagnostic messages - void check_voltage( diagnostic_updater::DiagnosticStatusWrapper &stat ); - void check_stall( diagnostic_updater::DiagnosticStatusWrapper &stat ); - - protected: - //! Node Handler used for publication of data. - ros::NodeHandle n; - //! Node Handler used for private data publication. - ros::NodeHandle nh_private; - - diagnostic_updater::Updater diagnostic_; - - diagnostic_updater::DiagnosedPublisher batt_pub_; - ros::Publisher mstate_pub_, grip_state_pub_, - ptz_state_pub_, sonar_pub_, aio_pub_, dio_pub_; - ros::Publisher pose_pub_; - ros::Subscriber cmdvel_sub_, cmdmstate_sub_, gripper_sub_, ptz_cmd_sub_; - - tf::TransformBroadcaster odom_broadcaster; - ros::Time veltime; - - SIP* sippacket; - std::string psos_serial_port; - std::string psos_tcp_host; - std::string odom_frame_id; - std::string base_link_frame_id; - int psos_fd; - bool psos_use_tcp; - int psos_tcp_port; - bool vel_dirty, motor_dirty; - bool gripper_dirty_; - int param_idx; - // PID settings - int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki; - - //! Stall I hit a wall? - int bumpstall; // should we change the bumper-stall behavior? - //! Use Joystick? - int joystick; - //! Control wheel velocities individually? - int direct_wheel_vel_control; - int radio_modemp; - - //! Maximum motor speed in Meters per second. - int motor_max_speed; - //! Maximum turn speed in radians per second. - int motor_max_turnspeed; - //! Maximum translational acceleration in Meters per second per second. - short motor_max_trans_accel; - //! Minimum translational acceleration in Meters per second per second. - short motor_max_trans_decel; - //! Maximum rotational acceleration in radians per second per second. - short motor_max_rot_accel; - //! Minimum rotational acceleration in Meters per second per second. - short motor_max_rot_decel; - //! Pulse time - double pulse; - double desired_freq; - //! Last time the node received or sent a pulse. - double lastPulseTime; - //! Use the sonar array? - bool use_sonar_; - - P2OSPtz ptz_; - - public: - //! Command Velocity subscriber - geometry_msgs::Twist cmdvel_; - //! Motor state publisher - p2os_msgs::MotorState cmdmotor_state_; - //! Gripper state publisher - p2os_msgs::GripperState gripper_state_; - //! sensor data container - ros_p2os_data_t p2os_data; + /*! \brief P2OS robot driver node. + * + * This class contains, essentially, the main means of communication + * between the robot and ROS. + */ +public: + P2OSNode(std::shared_ptr _node); + virtual ~P2OSNode(); + + //! Setup the robot for use. Communicates with the robot directly. + int Setup(); + //! Prepare for shutdown. + int Shutdown(); + + int SendReceive(P2OSPacket* pkt, bool publish_data = true ); + + void updateDiagnostics(); + + void ResetRawPositions(); + void ToggleSonarPower(unsigned char val); + void ToggleMotorPower(unsigned char val); + void StandardSIPPutData(tf2::TimePoint & ts); + + inline double TicksToDegrees (int joint, unsigned char ticks); + inline unsigned char DegreesToTicks (int joint, double degrees); + inline double TicksToRadians (int joint, unsigned char ticks); + inline unsigned char RadiansToTicks (int joint, double rads); + inline double RadsPerSectoSecsPerTick (int joint, double speed); + inline double SecsPerTicktoRadsPerSec (int joint, double secs); + + void SendPulse (void); + //void spin(); + void check_and_set_vel(); + void cmdvel_cb(const std::shared_ptr); + + void check_and_set_motor_state(); + void cmdmotor_state(const std::shared_ptr); + + void check_and_set_gripper_state(); + void gripperCallback(const std::shared_ptr node; + + /* TODO(allenh1): replace this with ROS2's version of dynamic_reconfigure */ + /* diagnostic_updater::Updater diagnostic_; */ + /* diagnostic_updater::DiagnosedPublisher batt_pub_; */ + + /* declare publishers */ + rclcpp::Publisher mstate_pub_; + rclcpp::Publisher grip_state_pub_; + rclcpp::Publisher ptz_state_pub_; + rclcpp::Publisher sonar_pub_; + rclcpp::Publisher aio_pub_; + rclcpp::Publisher dio_pub_; + rclcpp::Publisher pose_pub_; + + /* declare subscribers */ + rclcpp::Subscription cmdvel_sub_; + rclcpp::Subscription cmdmstate_sub_; + rclcpp::Subscription gripper_sub_; + rclcpp::Subscription ptz_cmd_sub_; + + tf2_ros::Buffer * buffer = nullptr; + tf2_ros::TransformBroadcaster * odom_broadcaster = nullptr; + tf2::TimePoint veltime; + + SIP* sippacket; + std::string psos_serial_port; + std::string psos_tcp_host; + std::string odom_frame_id; + std::string base_link_frame_id; + int psos_fd; + bool psos_use_tcp; + int psos_tcp_port; + bool vel_dirty, motor_dirty; + bool gripper_dirty_ = false; + int param_idx; + // PID settings + int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki; + + //! Stall I hit a wall? + int bumpstall; // should we change the bumper-stall behavior? + //! Use Joystick? + int joystick; + //! Control wheel velocities individually? + int direct_wheel_vel_control; + int radio_modemp; + + //! Maximum motor speed in Meters per second. + int motor_max_speed; + //! Maximum turn speed in radians per second. + int motor_max_turnspeed; + //! Maximum translational acceleration in Meters per second per second. + short motor_max_trans_accel; + //! Minimum translational acceleration in Meters per second per second. + short motor_max_trans_decel; + //! Maximum rotational acceleration in radians per second per second. + short motor_max_rot_accel; + //! Minimum rotational acceleration in Meters per second per second. + short motor_max_rot_decel; + //! Pulse time + double pulse; + double desired_freq; + //! Last time the node received or sent a pulse. + double lastPulseTime; + //! Use the sonar array? + bool use_sonar_; + + P2OSPtz ptz_; }; #endif diff --git a/p2os_driver/include/p2os_driver/p2os_ptz.h b/p2os_driver/include/p2os_driver/p2os_ptz.h index adc389c..bb43ad6 100644 --- a/p2os_driver/include/p2os_driver/p2os_ptz.h +++ b/p2os_driver/include/p2os_driver/p2os_ptz.h @@ -1,10 +1,14 @@ /* * P2OS for ROS + * Copyright (C) 2004, 2005 + * ActivMedia Robotics LLC + * Copyright (C) 2006, 2007, 2008, 2009 + * MobileRobots Inc. * Copyright (C) 2010 * Tucker Hermans, David Feil-Seifer, Brian Gerkey, Kasper Stoy, * Richard Vaughan, & Andrew Howard - * Copyright (C) 2004, 2005 ActivMedia Robotics LLC - * Copyright (C) 2006, 2007, 2008, 2009 MobileRobots Inc. + * Copyright (C) 2017 + * Open Source Robotics Foundation, Inc. * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -24,9 +28,9 @@ #ifndef _P2OS_PTZ_H #define _P2OS_PTZ_H -#include -#include +#include #include +#include class P2OSNode; diff --git a/p2os_driver/include/p2os_driver/packet.h b/p2os_driver/include/p2os_driver/packet.h index 2c02484..91b0402 100644 --- a/p2os_driver/include/p2os_driver/packet.h +++ b/p2os_driver/include/p2os_driver/packet.h @@ -3,7 +3,8 @@ * Copyright (C) 2009 * David Feil-Seifer, Brian Gerkey, Kasper Stoy, * Richard Vaughan, & Andrew Howard - * + * Copyright (C) 2017 + * Open Source Robotics Foundation, Inc * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -25,8 +26,9 @@ #ifndef _PACKET_H #define _PACKET_H -#include -#include +#include + +#include #define PACKET_LEN 256 diff --git a/p2os_driver/package.xml b/p2os_driver/package.xml index 468c724..ef82134 100644 --- a/p2os_driver/package.xml +++ b/p2os_driver/package.xml @@ -24,16 +24,24 @@ ActivMedia Robotics LLC MobileRobots Inc. - catkin + ament_cmake_auto + ament_cmake_ros std_msgs p2os_msgs geometry_msgs - tf - roscpp + tf2 + tf2_ros + tf2_msgs + tf2_srvs + tf2_geometry_msgs + rclcpp + rcutils nav_msgs - diagnostic_updater - kdl_parser + + - message_runtime + + ament_cmake + diff --git a/p2os_driver/src/p2os.cc b/p2os_driver/src/p2os.cc index 3ce571b..2ed2fdc 100644 --- a/p2os_driver/src/p2os.cc +++ b/p2os_driver/src/p2os.cc @@ -1,9 +1,10 @@ /* * P2OS for ROS * Copyright (C) 2009 - * Hunter Allen, David Feil-Seifer, Brian Gerkey, Kasper Stoy, + * David Feil-Seifer, Brian Gerkey, Kasper Stoy, * Richard Vaughan, & Andrew Howard - * + * Copyright (C) 2017 + * Open Source Robotics Foundation, inc. * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -24,660 +25,657 @@ #include #include -#include #include -P2OSNode::P2OSNode(ros::NodeHandle nh) : - n(nh), gripper_dirty_(false), - batt_pub_(n.advertise("battery_state",1000), - diagnostic_, - diagnostic_updater::FrequencyStatusParam(&desired_freq, &desired_freq, 0.1), - diagnostic_updater::TimeStampStatusParam()), - ptz_(this) +P2OSNode::P2OSNode(std::shared_ptr _node) +: node(_node), ptz_(this) { - /*! \brief Constructor for P2OS node. - * - * This brings up the P2OS system for ROS operation. - */ - - // Use sonar - ros::NodeHandle n_private("~"); - n_private.param(std::string("odom_frame_id"), odom_frame_id,std::string("odom")); - n_private.param(std::string("base_link_frame_id"), base_link_frame_id,std::string("base_link")); - n_private.param("use_sonar", use_sonar_, false); - - // read in config options - // bumpstall - n_private.param("bumpstall", bumpstall, -1); - // pulse - n_private.param("pulse", pulse, -1.0); - // rot_kp - n_private.param("rot_kp", rot_kp, -1); - // rot_kv - n_private.param("rot_kv", rot_kv, -1); - // rot_ki - n_private.param("rot_ki", rot_ki, -1); - // trans_kp - n_private.param("trans_kp", trans_kp, -1); - // trans_kv - n_private.param("trans_kv", trans_kv, -1); - // trans_ki - n_private.param("trans_ki", trans_ki, -1); - // !!! port !!! - std::string def = DEFAULT_P2OS_PORT; - n_private.param("port", psos_serial_port, def); - ROS_INFO("using serial port: [%s]", psos_serial_port.c_str()); - n_private.param("use_tcp", psos_use_tcp, false); - std::string host = DEFAULT_P2OS_TCP_REMOTE_HOST; - n_private.param("tcp_remote_host", psos_tcp_host, host); - n_private.param("tcp_remote_port", psos_tcp_port, DEFAULT_P2OS_TCP_REMOTE_PORT); - // radio - n_private.param("radio", radio_modemp, 0); - // joystick - n_private.param("joystick", joystick, 0); - // direct_wheel_vel_control - n_private.param("direct_wheel_vel_control", direct_wheel_vel_control, 0); - // max xpeed - double spd; - n_private.param("max_xspeed", spd, MOTOR_DEF_MAX_SPEED); - motor_max_speed = (int)rint(1e3*spd); - // max_yawspeed - n_private.param("max_yawspeed", spd, MOTOR_DEF_MAX_TURNSPEED); - motor_max_turnspeed = (short)rint(RTOD(spd)); - // max_xaccel - n_private.param("max_xaccel", spd, 0.0); - motor_max_trans_accel = (short)rint(1e3*spd); - // max_xdecel - n_private.param("max_xdecel", spd, 0.0); - motor_max_trans_decel = (short)rint(1e3*spd); - // max_yawaccel - n_private.param("max_yawaccel", spd, 0.0); - motor_max_rot_accel = (short)rint(RTOD(spd)); - // max_yawdecel - n_private.param("max_yawdecel", spd, 0.0); - motor_max_rot_decel = (short)rint(RTOD(spd)); - - desired_freq = 10; - - // advertise services - pose_pub_ = n.advertise("pose", 1000); - //advertise topics - mstate_pub_ = n.advertise("motor_state",1000); - grip_state_pub_ = n.advertise("gripper_state",1000); - ptz_state_pub_ = n.advertise("ptz_state",1000); - sonar_pub_ = n.advertise("sonar", 1000); - aio_pub_ = n.advertise("aio", 1000); - dio_pub_ = n.advertise("dio", 1000); - - // subscribe to services - cmdvel_sub_ = n.subscribe("cmd_vel", 1, &P2OSNode::cmdvel_cb, this); - cmdmstate_sub_ = n.subscribe("cmd_motor_state", 1, &P2OSNode::cmdmotor_state, - this); - gripper_sub_ = n.subscribe("gripper_control", 1, &P2OSNode::gripperCallback, - this); - ptz_cmd_sub_ = n.subscribe("ptz_control", 1, &P2OSPtz::callback, &ptz_); - - veltime = ros::Time::now(); - - // add diagnostic functions - diagnostic_.add("Motor Stall", this, &P2OSNode::check_stall); - diagnostic_.add("Battery Voltage", this, &P2OSNode::check_voltage); - - // initialize robot parameters (player legacy) - initialize_robot_params(); + /*! \brief Constructor for P2OS node. + * + * This brings up the P2OS system for ROS operation. + */ + node->get_parameter_or("odom_frame_id", odom_frame_id, std::string("odom")); + node->get_parameter_or("base_link_grame_id", base_link_frame_id, std::string("base_link")); + node->get_parameter_or("use_sonar", use_sonar_, false); + + // read in config options + // bumpstall + node->get_parameter_or("bumpstall", bumpstall, -1); + // pulse + node->get_parameter_or("pulse", pulse, -1.0); + // rot_kp + node->get_parameter_or("rot_kp", rot_kp, -1); + // rot_kv + node->get_parameter_or("rot_kv", rot_kv, -1); + // rot_ki + node->get_parameter_or("rot_ki", rot_ki, -1); + // trans_kp + node->get_parameter_or("trans_kp", trans_kp, -1); + // trans_kv + node->get_parameter_or("trans_kv", trans_kv, -1); + // trans_ki + node->get_parameter_or("trans_ki", trans_ki, -1); + // !!! port !!! + node->get_parameter_or("port", psos_serial_port, std::string(DEFAULT_P2OS_PORT)); + RCUTILS_LOG_INFO("using serial port: [%s]", psos_serial_port.c_str()); + node->get_parameter_or("use_tcp", psos_use_tcp, false); + std::string host(DEFAULT_P2OS_TCP_REMOTE_HOST); + node->get_parameter_or("tcp_remote_host", psos_tcp_host, host); + node->get_parameter_or("tcp_remote_port", psos_tcp_port, DEFAULT_P2OS_TCP_REMOTE_PORT); + // radio + node->get_parameter_or("radio", radio_modemp, 0); + // joystick + node->get_parameter_or("joystick", joystick, 0); + // direct_wheel_vel_control + node->get_parameter_or("direct_wheel_vel_control", direct_wheel_vel_control, 0); + // max xpeed + double spd; + n_private.param("max_xspeed", spd, MOTOR_DEF_MAX_SPEED); + node->get_parameter_or("max_xspeed", spd, MOTOR_DEF_MAX_SPEED); + motor_max_speed = (int)rint(1e3*spd); + // max_yawspeed + node->get_parameter_or("max_yawspeed", spd, MOTOR_DEF_MAX_TURNSPEED); + motor_max_turnspeed = (short)rint(RTOD(spd)); + // max_xaccel + node->get_parameter_or("max_xaccel", spd, 0.0); + motor_max_trans_accel = (short)rint(1e3*spd); + // max_xdecel + node->get_parameter_or("max_xdecel", spd, 0.0); + motor_max_trans_decel = (short)rint(1e3*spd); + // max_yawaccel + node->get_parameter_or("max_yawaccel", spd, 0.0); + motor_max_rot_accel = (short)rint(RTOD(spd)); + // max_yawdecel + node->get_parameter_or("max_yawdecel", spd, 0.0); + motor_max_rot_decel = (short)rint(RTOD(spd)); + + desired_freq = 10; + + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = 1000; /* TODO(allenh1): is this necessary? */ + qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + /* advertise topics */ + pose_pub_ = node->create_publisher("pose", qos); + mstate_pub_ = node->create_publisher("motor_state", qos); + grip_state_pub_ = node->create_publisher("gripper_state", qos); + ptz_state_pub_ = node->create_publisher("ptz_state", qos); + sonar_pub_ = node->create_publisher("sonar", qos); + aio_pub_ = node->create_publisher("aio", qos); + dio_pub_ = node->create_publisher("dio", qos); + + qos = rmw_qos_profile_default; + qos.depth = 1; /* TODO(allenh1): is this value too small? */ + + /* subscribe to topics */ + cmdvel_sub_ = node->create_subscription( + "cmd_vel", std::bind(&P2OSNode::cmdvel_cb, this, std::placeholders::_1), qos); + cmdmstate_sub_ = node->create_subscription( + "cmd_motor_state", std::bind(&P2OSNode::cmdmotor_state, this, std::placeholders::_1), qos); + gripper_sub_ = node->create_subscription( + "gripper_control", std::bind(&P2OSNode::gripperCallback, this, std::placeholders::_1), qos); + ptz_cmd_sub_ = node->create_subscription( + "ptz_control", std::bind(&P2OSPtz::callback, &ptz_, std::placeholders::_1), qos); + veltime = tf2_ros::toMsg(rclcpp::Time::now()); + + // add diagnostic functions + /* diagnostic_.add("Motor Stall", this, &P2OSNode::check_stall); */ + /* diagnostic_.add("Battery Voltage", this, &P2OSNode::check_voltage); */ + + // initialize robot parameters (player legacy) + initialize_robot_params(); } P2OSNode::~P2OSNode() {/** Destructor **/} -void P2OSNode::cmdmotor_state(const p2os_msgs::MotorStateConstPtr & msg) +void P2OSNode::cmdmotor_state(const std::shared_ptr msg) { - motor_dirty = true; - cmdmotor_state_ = *msg; + motor_dirty = true; + cmdmotor_state_ = *msg; } void P2OSNode::check_and_set_motor_state() { - if(!motor_dirty) return; - motor_dirty = false; - - unsigned char val = (unsigned char) cmdmotor_state_.state; - unsigned char command[4]; - P2OSPacket packet; - command[0] = ENABLE; - command[1] = ARGINT; - command[2] = val; - command[3] = 0; - packet.Build(command,4); - - // Store the current motor state so that we can set it back - p2os_data.motors.state = cmdmotor_state_.state; - SendReceive(&packet,false); + if(!motor_dirty) return; + motor_dirty = false; + + unsigned char val = (unsigned char) cmdmotor_state_.state; + unsigned char command[4]; + P2OSPacket packet; + command[0] = ENABLE; + command[1] = ARGINT; + command[2] = val; + command[3] = 0; + packet.Build(command,4); + + // Store the current motor state so that we can set it back + p2os_data.motors.state = cmdmotor_state_.state; + SendReceive(&packet,false); } void P2OSNode::check_and_set_gripper_state() { - if(!gripper_dirty_) return; - gripper_dirty_ = false; - - // Send the gripper command - unsigned char grip_val = (unsigned char) gripper_state_.grip.state; - unsigned char grip_command[4]; - - P2OSPacket grip_packet; - grip_command[0] = GRIPPER; - grip_command[1] = ARGINT; - grip_command[2] = grip_val; - grip_command[3] = 0; - grip_packet.Build(grip_command,4); - SendReceive(&grip_packet, false); - - // Send the lift command - unsigned char lift_val = (unsigned char) gripper_state_.lift.state; - unsigned char lift_command[4]; - - P2OSPacket lift_packet; - lift_command[0] = GRIPPER; - lift_command[1] = ARGINT; - lift_command[2] = lift_val; - lift_command[3] = 0; - lift_packet.Build(lift_command,4); - - SendReceive(&lift_packet,false); + if(!gripper_dirty_) return; + gripper_dirty_ = false; + + // Send the gripper command + unsigned char grip_val = (unsigned char) gripper_state_.grip.state; + unsigned char grip_command[4]; + + P2OSPacket grip_packet; + grip_command[0] = GRIPPER; + grip_command[1] = ARGINT; + grip_command[2] = grip_val; + grip_command[3] = 0; + grip_packet.Build(grip_command,4); + SendReceive(&grip_packet, false); + + // Send the lift command + unsigned char lift_val = (unsigned char) gripper_state_.lift.state; + unsigned char lift_command[4]; + + P2OSPacket lift_packet; + lift_command[0] = GRIPPER; + lift_command[1] = ARGINT; + lift_command[2] = lift_val; + lift_command[3] = 0; + lift_packet.Build(lift_command,4); + + SendReceive(&lift_packet,false); } -void P2OSNode::cmdvel_cb(const geometry_msgs::TwistConstPtr &msg) +void P2OSNode::cmdvel_cb(const std::shared_ptr msg) { - if(fabs(msg->linear.x - cmdvel_.linear.x) > 0.01 || fabs(msg->angular.z - cmdvel_.angular.z) > 0.01) { - veltime = ros::Time::now(); - ROS_DEBUG("new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec()); - vel_dirty = true; - cmdvel_ = *msg; - } else { - ros::Duration veldur = ros::Time::now() - veltime; - if(veldur.toSec() > 2.0 && ((fabs(cmdvel_.linear.x) > 0.01) || (fabs(cmdvel_.angular.z) > 0.01))) { - ROS_DEBUG("maintaining old speed: %0.3f|%0.3f", veltime.toSec(), ros::Time::now().toSec()); - vel_dirty = true; - veltime = ros::Time::now(); - } - } + if(fabs(msg->linear.x - cmdvel_.linear.x) > 0.01 || fabs(msg->angular.z - cmdvel_.angular.z) > 0.01) { + veltime = ros::Time::now(); + ROS_DEBUG("new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec()); + vel_dirty = true; + cmdvel_ = *msg; + } else { + ros::Duration veldur = ros::Time::now() - veltime; + if(veldur.toSec() > 2.0 && ((fabs(cmdvel_.linear.x) > 0.01) || (fabs(cmdvel_.angular.z) > 0.01))) { + ROS_DEBUG("maintaining old speed: %0.3f|%0.3f", veltime.toSec(), ros::Time::now().toSec()); + vel_dirty = true; + veltime = ros::Time::now(); + } + } } void P2OSNode::check_and_set_vel() { - if(!vel_dirty) return; + if(!vel_dirty) return; - ROS_DEBUG("setting vel: [%0.2f,%0.2f]",cmdvel_.linear.x,cmdvel_.angular.z); - vel_dirty = false; + ROS_DEBUG("setting vel: [%0.2f,%0.2f]",cmdvel_.linear.x,cmdvel_.angular.z); + vel_dirty = false; - unsigned short absSpeedDemand, absturnRateDemand; - unsigned char motorcommand[4]; - P2OSPacket motorpacket; + unsigned short absSpeedDemand, absturnRateDemand; + unsigned char motorcommand[4]; + P2OSPacket motorpacket; - int vx = (int) (cmdvel_.linear.x*1e3); - int va = (int) rint(RTOD(cmdvel_.angular.z)); + int vx = (int) (cmdvel_.linear.x*1e3); + int va = (int) rint(RTOD(cmdvel_.angular.z)); - // non-direct wheel control - motorcommand[0] = VEL; - motorcommand[1] = (vx >= 0) ? ARGINT : ARGNINT; + // non-direct wheel control + motorcommand[0] = VEL; + motorcommand[1] = (vx >= 0) ? ARGINT : ARGNINT; - absSpeedDemand = (unsigned short) abs(vx); + absSpeedDemand = (unsigned short) abs(vx); - if(absSpeedDemand <= this->motor_max_speed) { - motorcommand[2] = absSpeedDemand & 0x00FF; - motorcommand[3] = (absSpeedDemand & 0xFF00) >> 8; - } else { - ROS_WARN("speed demand thresholded! (true: %u, max: %u)", absSpeedDemand, motor_max_speed); - motorcommand[2] = motor_max_speed & 0x00FF; - motorcommand[3] = (motor_max_speed & 0xFF00) >> 8; - } + if(absSpeedDemand <= this->motor_max_speed) { + motorcommand[2] = absSpeedDemand & 0x00FF; + motorcommand[3] = (absSpeedDemand & 0xFF00) >> 8; + } else { + ROS_WARN("speed demand thresholded! (true: %u, max: %u)", absSpeedDemand, motor_max_speed); + motorcommand[2] = motor_max_speed & 0x00FF; + motorcommand[3] = (motor_max_speed & 0xFF00) >> 8; + } - motorpacket.Build(motorcommand, 4); - SendReceive(&motorpacket); + motorpacket.Build(motorcommand, 4); + SendReceive(&motorpacket); - motorcommand[0] = RVEL; - motorcommand[1] = (va >= 0) ? ARGINT : ARGNINT; - - absturnRateDemand = (unsigned short) (va >= 0) ? va : (-1 * va); - - if(absturnRateDemand <= motor_max_turnspeed) { - motorcommand[2] = absturnRateDemand & 0x00FF; - motorcommand[3] = (absturnRateDemand & 0xFF00) >> 8; - } else { - ROS_WARN("Turn rate demand threshholded!"); - motorcommand[2] = this->motor_max_turnspeed & 0x00FF; - motorcommand[3] = (this->motor_max_turnspeed & 0xFF00) >> 8; - } + motorcommand[0] = RVEL; + motorcommand[1] = (va >= 0) ? ARGINT : ARGNINT; + + absturnRateDemand = (unsigned short) (va >= 0) ? va : (-1 * va); + + if(absturnRateDemand <= motor_max_turnspeed) { + motorcommand[2] = absturnRateDemand & 0x00FF; + motorcommand[3] = (absturnRateDemand & 0xFF00) >> 8; + } else { + ROS_WARN("Turn rate demand threshholded!"); + motorcommand[2] = this->motor_max_turnspeed & 0x00FF; + motorcommand[3] = (this->motor_max_turnspeed & 0xFF00) >> 8; + } - motorpacket.Build(motorcommand,4); - SendReceive(&motorpacket); + motorpacket.Build(motorcommand,4); + SendReceive(&motorpacket); } void P2OSNode::gripperCallback(const p2os_msgs::GripperStateConstPtr &msg) { - gripper_dirty_ = true; - gripper_state_ = *msg; + gripper_dirty_ = true; + gripper_state_ = *msg; } int P2OSNode::Setup() { - int i; - int bauds[] = {B9600, B38400, B19200, B115200, B57600}; - int numbauds = sizeof(bauds); - int currbaud = 0; - sippacket = NULL; - lastPulseTime = 0.0; - - struct termios term; - unsigned char command; - P2OSPacket packet, receivedpacket; - int flags=0; - bool sent_close = false; - - enum - { - NO_SYNC, - AFTER_FIRST_SYNC, - AFTER_SECOND_SYNC, - READY + int i; + int bauds[] = {B9600, B38400, B19200, B115200, B57600}; + int numbauds = sizeof(bauds); + int currbaud = 0; + sippacket = NULL; + lastPulseTime = 0.0; + + struct termios term; + unsigned char command; + P2OSPacket packet, receivedpacket; + int flags=0; + bool sent_close = false; + + enum + { + NO_SYNC, + AFTER_FIRST_SYNC, + AFTER_SECOND_SYNC, + READY } psos_state; - psos_state = NO_SYNC; - - char name[20], type[20], subtype[20]; - int cnt; - - - // use serial port - - ROS_INFO("P2OS connection opening serial port %s...",psos_serial_port.c_str()); - - if((this->psos_fd = open(this->psos_serial_port.c_str(), - O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR)) < 0) + psos_state = NO_SYNC; + + char name[20], type[20], subtype[20]; + int cnt; + + + // use serial port + + ROS_INFO("P2OS connection opening serial port %s...",psos_serial_port.c_str()); + + if((this->psos_fd = open(this->psos_serial_port.c_str(), + O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR)) < 0) + { + ROS_ERROR("P2OS::Setup():open():"); + return(1); + } + + if(tcgetattr(this->psos_fd, &term) < 0) + { + ROS_ERROR("P2OS::Setup():tcgetattr():"); + close(this->psos_fd); + this->psos_fd = -1; + return(1); + } + + cfmakeraw(&term); + cfsetispeed(&term, bauds[currbaud]); + cfsetospeed(&term, bauds[currbaud]); + + if(tcsetattr(this->psos_fd, TCSAFLUSH, &term) < 0) + { + ROS_ERROR("P2OS::Setup():tcsetattr():"); + close(this->psos_fd); + this->psos_fd = -1; + return(1); + } + + if(tcflush(this->psos_fd, TCIOFLUSH) < 0) + { + ROS_ERROR("P2OS::Setup():tcflush():"); + close(this->psos_fd); + this->psos_fd = -1; + return(1); + } + + if((flags = fcntl(this->psos_fd, F_GETFL)) < 0) + { + ROS_ERROR("P2OS::Setup():fcntl()"); + close(this->psos_fd); + this->psos_fd = -1; + return(1); + } + // Sync: + + int num_sync_attempts = 3; + while(psos_state != READY) + { + switch(psos_state) { - ROS_ERROR("P2OS::Setup():open():"); - return(1); - } - - if(tcgetattr(this->psos_fd, &term) < 0) - { - ROS_ERROR("P2OS::Setup():tcgetattr():"); - close(this->psos_fd); - this->psos_fd = -1; - return(1); - } - - cfmakeraw(&term); - cfsetispeed(&term, bauds[currbaud]); - cfsetospeed(&term, bauds[currbaud]); - - if(tcsetattr(this->psos_fd, TCSAFLUSH, &term) < 0) - { - ROS_ERROR("P2OS::Setup():tcsetattr():"); - close(this->psos_fd); - this->psos_fd = -1; - return(1); - } - - if(tcflush(this->psos_fd, TCIOFLUSH) < 0) - { - ROS_ERROR("P2OS::Setup():tcflush():"); - close(this->psos_fd); - this->psos_fd = -1; - return(1); - } - - if((flags = fcntl(this->psos_fd, F_GETFL)) < 0) - { - ROS_ERROR("P2OS::Setup():fcntl()"); - close(this->psos_fd); - this->psos_fd = -1; - return(1); - } - // Sync: - - int num_sync_attempts = 3; - while(psos_state != READY) - { - switch(psos_state) + case NO_SYNC: + command = SYNC0; + packet.Build(&command, 1); + packet.Send(this->psos_fd); + usleep(P2OS_CYCLETIME_USEC); + break; + case AFTER_FIRST_SYNC: + ROS_INFO("turning off NONBLOCK mode..."); + if(fcntl(this->psos_fd, F_SETFL, flags ^ O_NONBLOCK) < 0) { - case NO_SYNC: - command = SYNC0; - packet.Build(&command, 1); - packet.Send(this->psos_fd); - usleep(P2OS_CYCLETIME_USEC); - break; - case AFTER_FIRST_SYNC: - ROS_INFO("turning off NONBLOCK mode..."); - if(fcntl(this->psos_fd, F_SETFL, flags ^ O_NONBLOCK) < 0) - { - ROS_ERROR("P2OS::Setup():fcntl()"); - close(this->psos_fd); - this->psos_fd = -1; - return(1); - } - command = SYNC1; - packet.Build(&command, 1); - packet.Send(this->psos_fd); - break; - case AFTER_SECOND_SYNC: - command = SYNC2; - packet.Build(&command, 1); - packet.Send(this->psos_fd); - break; - default: - ROS_WARN("P2OS::Setup():shouldn't be here..."); - break; + ROS_ERROR("P2OS::Setup():fcntl()"); + close(this->psos_fd); + this->psos_fd = -1; + return(1); } - usleep(P2OS_CYCLETIME_USEC); + command = SYNC1; + packet.Build(&command, 1); + packet.Send(this->psos_fd); + break; + case AFTER_SECOND_SYNC: + command = SYNC2; + packet.Build(&command, 1); + packet.Send(this->psos_fd); + break; + default: + ROS_WARN("P2OS::Setup():shouldn't be here..."); + break; + } + usleep(P2OS_CYCLETIME_USEC); - if(receivedpacket.Receive(this->psos_fd)) + if(receivedpacket.Receive(this->psos_fd)) + { + if((psos_state == NO_SYNC) && (num_sync_attempts >= 0)) + { + num_sync_attempts--; + usleep(P2OS_CYCLETIME_USEC); + continue; + } + else + { + // couldn't connect; try different speed. + if(++currbaud < numbauds) { - if((psos_state == NO_SYNC) && (num_sync_attempts >= 0)) - { - num_sync_attempts--; - usleep(P2OS_CYCLETIME_USEC); - continue; - } - else - { - // couldn't connect; try different speed. - if(++currbaud < numbauds) - { - cfsetispeed(&term, bauds[currbaud]); - cfsetospeed(&term, bauds[currbaud]); - if(tcsetattr(this->psos_fd, TCSAFLUSH, &term) < 0) - { - ROS_ERROR("P2OS::Setup():tcsetattr():"); - close(this->psos_fd); - this->psos_fd = -1; - return(1); - } + cfsetispeed(&term, bauds[currbaud]); + cfsetospeed(&term, bauds[currbaud]); + if(tcsetattr(this->psos_fd, TCSAFLUSH, &term) < 0) + { + ROS_ERROR("P2OS::Setup():tcsetattr():"); + close(this->psos_fd); + this->psos_fd = -1; + return(1); + } - if(tcflush(this->psos_fd, TCIOFLUSH) < 0) - { - ROS_ERROR("P2OS::Setup():tcflush():"); - close(this->psos_fd); - this->psos_fd = -1; - return(1); - } - num_sync_attempts = 3; - continue; - } - else - { - // tried all speeds; bail - break; - } - } + if(tcflush(this->psos_fd, TCIOFLUSH) < 0) + { + ROS_ERROR("P2OS::Setup():tcflush():"); + close(this->psos_fd); + this->psos_fd = -1; + return(1); + } + num_sync_attempts = 3; + continue; } - switch(receivedpacket.packet[3]) + else { - case SYNC0: - ROS_INFO("SYNC0"); - psos_state = AFTER_FIRST_SYNC; - break; - case SYNC1: - ROS_INFO("SYNC1"); - psos_state = AFTER_SECOND_SYNC; - break; - case SYNC2: - ROS_INFO("SYNC2"); - psos_state = READY; - break; - default: - // maybe P2OS is still running from last time. let's try to CLOSE - // and reconnect - if(!sent_close) - { - ROS_DEBUG("sending CLOSE"); - command = CLOSE; - packet.Build(&command, 1); - packet.Send(this->psos_fd); - sent_close = true; - usleep(2*P2OS_CYCLETIME_USEC); - tcflush(this->psos_fd,TCIFLUSH); - psos_state = NO_SYNC; - } - break; + // tried all speeds; bail + break; } - usleep(P2OS_CYCLETIME_USEC); - } - if(psos_state != READY) - { - if(this->psos_use_tcp) - ROS_INFO("Couldn't synchronize with P2OS.\n" - " Most likely because the robot is not connected %s %s", - this->psos_use_tcp ? "to the ethernet-serial bridge device " : "to the serial port", - this->psos_use_tcp ? this->psos_tcp_host.c_str() : this->psos_serial_port.c_str()); - close(this->psos_fd); - this->psos_fd = -1; - return(1); + } } - cnt = 4; - cnt += snprintf(name, sizeof(name), "%s", &receivedpacket.packet[cnt]); - cnt++; - cnt += snprintf(type, sizeof(type), "%s", &receivedpacket.packet[cnt]); - cnt++; - cnt += snprintf(subtype, sizeof(subtype), "%s", &receivedpacket.packet[cnt]); - cnt++; - - std::string hwID = std::string(name) + ": " + std::string(type) + "/" + std::string(subtype); - diagnostic_.setHardwareID(hwID); - - command = OPEN; - packet.Build(&command, 1); - packet.Send(this->psos_fd); - usleep(P2OS_CYCLETIME_USEC); - command = PULSE; - packet.Build(&command, 1); - packet.Send(this->psos_fd); - usleep(P2OS_CYCLETIME_USEC); - - ROS_INFO("Done.\n Connected to %s, a %s %s", name, type, subtype); - - // now, based on robot type, find the right set of parameters - for(i=0;ipsos_fd); + sent_close = true; + usleep(2*P2OS_CYCLETIME_USEC); + tcflush(this->psos_fd,TCIFLUSH); + psos_state = NO_SYNC; } + break; } - if(i == PLAYER_NUM_ROBOT_TYPES) + usleep(P2OS_CYCLETIME_USEC); + } + if(psos_state != READY) + { + if(this->psos_use_tcp) + ROS_INFO("Couldn't synchronize with P2OS.\n" + " Most likely because the robot is not connected %s %s", + this->psos_use_tcp ? "to the ethernet-serial bridge device " : "to the serial port", + this->psos_use_tcp ? this->psos_tcp_host.c_str() : this->psos_serial_port.c_str()); + close(this->psos_fd); + this->psos_fd = -1; + return(1); + } + cnt = 4; + cnt += snprintf(name, sizeof(name), "%s", &receivedpacket.packet[cnt]); + cnt++; + cnt += snprintf(type, sizeof(type), "%s", &receivedpacket.packet[cnt]); + cnt++; + cnt += snprintf(subtype, sizeof(subtype), "%s", &receivedpacket.packet[cnt]); + cnt++; + + std::string hwID = std::string(name) + ": " + std::string(type) + "/" + std::string(subtype); + diagnostic_.setHardwareID(hwID); + + command = OPEN; + packet.Build(&command, 1); + packet.Send(this->psos_fd); + usleep(P2OS_CYCLETIME_USEC); + command = PULSE; + packet.Build(&command, 1); + packet.Send(this->psos_fd); + usleep(P2OS_CYCLETIME_USEC); + + ROS_INFO("Done.\n Connected to %s, a %s %s", name, type, subtype); + + // now, based on robot type, find the right set of parameters + for(i=0;iodom_frame_id = odom_frame_id; - sippacket->base_link_frame_id = base_link_frame_id; - } - /* - sippacket->x_offset = 0; - sippacket->y_offset = 0; - sippacket->angle_offset = 0; + } + if(i == PLAYER_NUM_ROBOT_TYPES) + { + ROS_WARN("P2OS: Warning: couldn't find parameters for this robot; " + "using defaults"); + param_idx = 0; + } + + //sleep(1); + + // first, receive a packet so we know we're connected. + if(!sippacket) + { + sippacket = new SIP(param_idx); + sippacket->odom_frame_id = odom_frame_id; + sippacket->base_link_frame_id = base_link_frame_id; + } + /* + sippacket->x_offset = 0; + sippacket->y_offset = 0; + sippacket->angle_offset = 0; - SendReceive((P2OSPacket*)NULL,false); - */ - // turn off the sonars at first - this->ToggleSonarPower(0); - // if requested, set max accel/decel limits - P2OSPacket accel_packet; - unsigned char accel_command[4]; - if(this->motor_max_trans_accel > 0) + SendReceive((P2OSPacket*)NULL,false); + */ + // turn off the sonars at first + this->ToggleSonarPower(0); + // if requested, set max accel/decel limits + P2OSPacket accel_packet; + unsigned char accel_command[4]; + if(this->motor_max_trans_accel > 0) + { + accel_command[0] = SETA; + accel_command[1] = ARGINT; + accel_command[2] = this->motor_max_trans_accel & 0x00FF; + accel_command[3] = (this->motor_max_trans_accel & 0xFF00) >> 8; + accel_packet.Build(accel_command, 4); + this->SendReceive(&accel_packet,false); + } + + if(this->motor_max_trans_decel < 0) + { + accel_command[0] = SETA; + accel_command[1] = ARGNINT; + accel_command[2] = -1 *(this->motor_max_trans_decel) & 0x00FF; + accel_command[3] = (-1 *(this->motor_max_trans_decel) & 0xFF00) >> 8; + accel_packet.Build(accel_command, 4); + this->SendReceive(&accel_packet,false); + } + if(this->motor_max_rot_accel > 0) + { + accel_command[0] = SETRA; + accel_command[1] = ARGINT; + accel_command[2] = this->motor_max_rot_accel & 0x00FF; + accel_command[3] = (this->motor_max_rot_accel & 0xFF00) >> 8; + accel_packet.Build(accel_command, 4); + this->SendReceive(&accel_packet,false); + } + if(this->motor_max_rot_decel < 0) + { + accel_command[0] = SETRA; + accel_command[1] = ARGNINT; + accel_command[2] = -1*(this->motor_max_rot_decel) & 0x00FF; + accel_command[3] = (-1*(this->motor_max_rot_decel) & 0xFF00) >> 8; + accel_packet.Build(accel_command, 4); + this->SendReceive(&accel_packet,false); + } + + + // if requested, change PID settings + P2OSPacket pid_packet; + unsigned char pid_command[4]; + if(this->rot_kp >= 0) + { + pid_command[0] = ROTKP; + pid_command[1] = ARGINT; + pid_command[2] = this->rot_kp & 0x00FF; + pid_command[3] = (this->rot_kp & 0xFF00) >> 8; + pid_packet.Build(pid_command, 4); + this->SendReceive(&pid_packet); + } + if(this->rot_kv >= 0) + { + pid_command[0] = ROTKV; + pid_command[1] = ARGINT; + pid_command[2] = this->rot_kv & 0x00FF; + pid_command[3] = (this->rot_kv & 0xFF00) >> 8; + pid_packet.Build(pid_command, 4); + this->SendReceive(&pid_packet); + } + if(this->rot_ki >= 0) + { + pid_command[0] = ROTKI; + pid_command[1] = ARGINT; + pid_command[2] = this->rot_ki & 0x00FF; + pid_command[3] = (this->rot_ki & 0xFF00) >> 8; + pid_packet.Build(pid_command, 4); + this->SendReceive(&pid_packet); + } + if(this->trans_kp >= 0) + { + pid_command[0] = TRANSKP; + pid_command[1] = ARGINT; + pid_command[2] = this->trans_kp & 0x00FF; + pid_command[3] = (this->trans_kp & 0xFF00) >> 8; + pid_packet.Build(pid_command, 4); + this->SendReceive(&pid_packet); + } + if(this->trans_kv >= 0) + { + pid_command[0] = TRANSKV; + pid_command[1] = ARGINT; + pid_command[2] = this->trans_kv & 0x00FF; + pid_command[3] = (this->trans_kv & 0xFF00) >> 8; + pid_packet.Build(pid_command, 4); + this->SendReceive(&pid_packet); + } + if(this->trans_ki >= 0) + { + pid_command[0] = TRANSKI; + pid_command[1] = ARGINT; + pid_command[2] = this->trans_ki & 0x00FF; + pid_command[3] = (this->trans_ki & 0xFF00) >> 8; + pid_packet.Build(pid_command, 4); + this->SendReceive(&pid_packet); + } + + + // if requested, change bumper-stall behavior + // 0 = don't stall + // 1 = stall on front bumper contact + // 2 = stall on rear bumper contact + // 3 = stall on either bumper contact + if(this->bumpstall >= 0) + { + if(this->bumpstall > 3) + ROS_INFO("ignoring bumpstall value %d; should be 0, 1, 2, or 3", + this->bumpstall); + else { - accel_command[0] = SETA; - accel_command[1] = ARGINT; - accel_command[2] = this->motor_max_trans_accel & 0x00FF; - accel_command[3] = (this->motor_max_trans_accel & 0xFF00) >> 8; - accel_packet.Build(accel_command, 4); - this->SendReceive(&accel_packet,false); + ROS_INFO("setting bumpstall to %d", this->bumpstall); + P2OSPacket bumpstall_packet;; + unsigned char bumpstall_command[4]; + bumpstall_command[0] = BUMP_STALL; + bumpstall_command[1] = ARGINT; + bumpstall_command[2] = (unsigned char)this->bumpstall; + bumpstall_command[3] = 0; + bumpstall_packet.Build(bumpstall_command, 4); + this->SendReceive(&bumpstall_packet,false); } + } - if(this->motor_max_trans_decel < 0) - { - accel_command[0] = SETA; - accel_command[1] = ARGNINT; - accel_command[2] = -1 *(this->motor_max_trans_decel) & 0x00FF; - accel_command[3] = (-1 *(this->motor_max_trans_decel) & 0xFF00) >> 8; - accel_packet.Build(accel_command, 4); - this->SendReceive(&accel_packet,false); - } - if(this->motor_max_rot_accel > 0) - { - accel_command[0] = SETRA; - accel_command[1] = ARGINT; - accel_command[2] = this->motor_max_rot_accel & 0x00FF; - accel_command[3] = (this->motor_max_rot_accel & 0xFF00) >> 8; - accel_packet.Build(accel_command, 4); - this->SendReceive(&accel_packet,false); - } - if(this->motor_max_rot_decel < 0) - { - accel_command[0] = SETRA; - accel_command[1] = ARGNINT; - accel_command[2] = -1*(this->motor_max_rot_decel) & 0x00FF; - accel_command[3] = (-1*(this->motor_max_rot_decel) & 0xFF00) >> 8; - accel_packet.Build(accel_command, 4); - this->SendReceive(&accel_packet,false); - } - - - // if requested, change PID settings - P2OSPacket pid_packet; - unsigned char pid_command[4]; - if(this->rot_kp >= 0) - { - pid_command[0] = ROTKP; - pid_command[1] = ARGINT; - pid_command[2] = this->rot_kp & 0x00FF; - pid_command[3] = (this->rot_kp & 0xFF00) >> 8; - pid_packet.Build(pid_command, 4); - this->SendReceive(&pid_packet); - } - if(this->rot_kv >= 0) - { - pid_command[0] = ROTKV; - pid_command[1] = ARGINT; - pid_command[2] = this->rot_kv & 0x00FF; - pid_command[3] = (this->rot_kv & 0xFF00) >> 8; - pid_packet.Build(pid_command, 4); - this->SendReceive(&pid_packet); - } - if(this->rot_ki >= 0) - { - pid_command[0] = ROTKI; - pid_command[1] = ARGINT; - pid_command[2] = this->rot_ki & 0x00FF; - pid_command[3] = (this->rot_ki & 0xFF00) >> 8; - pid_packet.Build(pid_command, 4); - this->SendReceive(&pid_packet); - } - if(this->trans_kp >= 0) - { - pid_command[0] = TRANSKP; - pid_command[1] = ARGINT; - pid_command[2] = this->trans_kp & 0x00FF; - pid_command[3] = (this->trans_kp & 0xFF00) >> 8; - pid_packet.Build(pid_command, 4); - this->SendReceive(&pid_packet); - } - if(this->trans_kv >= 0) - { - pid_command[0] = TRANSKV; - pid_command[1] = ARGINT; - pid_command[2] = this->trans_kv & 0x00FF; - pid_command[3] = (this->trans_kv & 0xFF00) >> 8; - pid_packet.Build(pid_command, 4); - this->SendReceive(&pid_packet); - } - if(this->trans_ki >= 0) - { - pid_command[0] = TRANSKI; - pid_command[1] = ARGINT; - pid_command[2] = this->trans_ki & 0x00FF; - pid_command[3] = (this->trans_ki & 0xFF00) >> 8; - pid_packet.Build(pid_command, 4); - this->SendReceive(&pid_packet); - } - - - // if requested, change bumper-stall behavior - // 0 = don't stall - // 1 = stall on front bumper contact - // 2 = stall on rear bumper contact - // 3 = stall on either bumper contact - if(this->bumpstall >= 0) - { - if(this->bumpstall > 3) - ROS_INFO("ignoring bumpstall value %d; should be 0, 1, 2, or 3", - this->bumpstall); - else - { - ROS_INFO("setting bumpstall to %d", this->bumpstall); - P2OSPacket bumpstall_packet;; - unsigned char bumpstall_command[4]; - bumpstall_command[0] = BUMP_STALL; - bumpstall_command[1] = ARGINT; - bumpstall_command[2] = (unsigned char)this->bumpstall; - bumpstall_command[3] = 0; - bumpstall_packet.Build(bumpstall_command, 4); - this->SendReceive(&bumpstall_packet,false); - } - } + // Turn on the sonar + if(use_sonar_) { + this->ToggleSonarPower(1); + ROS_DEBUG("Sonar array powered on."); + } + ptz_.setup(); - // Turn on the sonar - if(use_sonar_) { - this->ToggleSonarPower(1); - ROS_DEBUG("Sonar array powered on."); - } - ptz_.setup(); - - return(0); + return(0); } int P2OSNode::Shutdown() { - unsigned char command[20],buffer[20]; - P2OSPacket packet; + unsigned char command[20],buffer[20]; + P2OSPacket packet; - if (ptz_.isOn()) - { - ptz_.shutdown(); - } + if (ptz_.isOn()) + { + ptz_.shutdown(); + } - memset(buffer,0,20); + memset(buffer,0,20); - if(this->psos_fd == -1) - return -1; + if(this->psos_fd == -1) + return -1; - command[0] = STOP; - packet.Build(command, 1); - packet.Send(this->psos_fd); - usleep(P2OS_CYCLETIME_USEC); + command[0] = STOP; + packet.Build(command, 1); + packet.Send(this->psos_fd); + usleep(P2OS_CYCLETIME_USEC); - command[0] = CLOSE; - packet.Build(command, 1); - packet.Send(this->psos_fd); - usleep(P2OS_CYCLETIME_USEC); + command[0] = CLOSE; + packet.Build(command, 1); + packet.Send(this->psos_fd); + usleep(P2OS_CYCLETIME_USEC); - close(this->psos_fd); - this->psos_fd = -1; - ROS_INFO("P2OS has been shutdown"); - delete this->sippacket; - this->sippacket = NULL; + close(this->psos_fd); + this->psos_fd = -1; + ROS_INFO("P2OS has been shutdown"); + delete this->sippacket; + this->sippacket = NULL; - return 0; + return 0; } @@ -685,149 +683,149 @@ void P2OSNode::StandardSIPPutData(ros::Time ts) { - p2os_data.position.header.stamp = ts; - pose_pub_.publish(p2os_data.position); - p2os_data.odom_trans.header.stamp = ts; - odom_broadcaster.sendTransform(p2os_data.odom_trans); + p2os_data.position.header.stamp = ts; + pose_pub_.publish(p2os_data.position); + p2os_data.odom_trans.header.stamp = ts; + odom_broadcaster.sendTransform(p2os_data.odom_trans); - p2os_data.batt.header.stamp = ts; - batt_pub_.publish(p2os_data.batt); - mstate_pub_.publish(p2os_data.motors); + p2os_data.batt.header.stamp = ts; + batt_pub_.publish(p2os_data.batt); + mstate_pub_.publish(p2os_data.motors); - // put sonar data - p2os_data.sonar.header.stamp = ts; - sonar_pub_.publish(p2os_data.sonar); + // put sonar data + p2os_data.sonar.header.stamp = ts; + sonar_pub_.publish(p2os_data.sonar); - // put aio data - aio_pub_.publish(p2os_data.aio); - // put dio data - dio_pub_.publish(p2os_data.dio); + // put aio data + aio_pub_.publish(p2os_data.aio); + // put dio data + dio_pub_.publish(p2os_data.dio); - // put gripper and lift data - grip_state_pub_.publish(p2os_data.gripper); - ptz_state_pub_.publish(ptz_.getCurrentState()); + // put gripper and lift data + grip_state_pub_.publish(p2os_data.gripper); + ptz_state_pub_.publish(ptz_.getCurrentState()); - // put bumper data - // put compass data + // put bumper data + // put compass data } /* send the packet, then receive and parse an SIP */ int P2OSNode::SendReceive(P2OSPacket* pkt, bool publish_data) { - P2OSPacket packet; + P2OSPacket packet; - if((this->psos_fd >= 0) && this->sippacket) - { - if(pkt) - pkt->Send(this->psos_fd); + if((this->psos_fd >= 0) && this->sippacket) + { + if(pkt) + pkt->Send(this->psos_fd); - /* receive a packet */ - pthread_testcancel(); - if(packet.Receive(this->psos_fd)) - { - ROS_ERROR("RunPsosThread(): Receive errored"); - pthread_exit(NULL); - } + /* receive a packet */ + pthread_testcancel(); + if(packet.Receive(this->psos_fd)) + { + ROS_ERROR("RunPsosThread(): Receive errored"); + pthread_exit(NULL); + } - if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && - (packet.packet[3] == 0x30 || packet.packet[3] == 0x31 || - packet.packet[3] == 0x32 || packet.packet[3] == 0x33 || - packet.packet[3] == 0x34)) - { + if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && + (packet.packet[3] == 0x30 || packet.packet[3] == 0x31 || + packet.packet[3] == 0x32 || packet.packet[3] == 0x33 || + packet.packet[3] == 0x34)) + { - /* It is a server packet, so process it */ - this->sippacket->ParseStandard(&packet.packet[3]); - this->sippacket->FillStandard(&(this->p2os_data)); + /* It is a server packet, so process it */ + this->sippacket->ParseStandard(&packet.packet[3]); + this->sippacket->FillStandard(&(this->p2os_data)); - if(publish_data) - this->StandardSIPPutData(packet.timestamp); - } - else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && - packet.packet[3] == SERAUX) + if(publish_data) + this->StandardSIPPutData(packet.timestamp); + } + else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB && + packet.packet[3] == SERAUX) + { + // This is an AUX serial packet + if(ptz_.isOn()) + { + int len = packet.packet[2] - 3; + if (ptz_.cb_.gotPacket()) { - // This is an AUX serial packet - if(ptz_.isOn()) - { - int len = packet.packet[2] - 3; - if (ptz_.cb_.gotPacket()) - { - ROS_ERROR("PTZ got a message, but alread has the complete packet."); - } - else - { - for (int i=4; i < 4+len; ++i) - { - ptz_.cb_.putOnBuf(packet.packet[i]); - } - } - } + ROS_ERROR("PTZ got a message, but alread has the complete packet."); } - else + else { - ROS_ERROR("Received other packet!"); - packet.PrintHex(); + for (int i=4; i < 4+len; ++i) + { + ptz_.cb_.putOnBuf(packet.packet[i]); + } } + } + } + else + { + ROS_ERROR("Received other packet!"); + packet.PrintHex(); } + } - return(0); + return(0); } void P2OSNode::updateDiagnostics() { - diagnostic_.update(); + diagnostic_.update(); } void P2OSNode::check_voltage(diagnostic_updater::DiagnosticStatusWrapper &stat) { - double voltage = sippacket->battery / 10.0; - if(voltage < 11.0) stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "battery voltage critically low"); - else if (voltage < 11.75) stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "battery voltage getting low"); - else stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "battery voltage OK"); + double voltage = sippacket->battery / 10.0; + if(voltage < 11.0) stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "battery voltage critically low"); + else if (voltage < 11.75) stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "battery voltage getting low"); + else stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "battery voltage OK"); - stat.add("voltage", voltage); + stat.add("voltage", voltage); } void P2OSNode::check_stall(diagnostic_updater::DiagnosticStatusWrapper &stat) { - if(sippacket->lwstall||sippacket->rwstall) stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, - "wheel stalled"); - else stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "no wheel stall"); + if(sippacket->lwstall||sippacket->rwstall) stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, + "wheel stalled"); + else stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "no wheel stall"); - stat.add("left wheel stall", sippacket->lwstall); - stat.add("right wheel stall", sippacket->rwstall); + stat.add("left wheel stall", sippacket->lwstall); + stat.add("right wheel stall", sippacket->rwstall); } void P2OSNode::ResetRawPositions() { - P2OSPacket pkt; - unsigned char p2oscommand[4]; - - if(this->sippacket) { - this->sippacket->rawxpos = 0; - this->sippacket->rawypos = 0; - this->sippacket->xpos = 0; - this->sippacket->ypos = 0; - p2oscommand[0] = SETO; - p2oscommand[1] = ARGINT; - pkt.Build(p2oscommand, 2); - this->SendReceive(&pkt,false); - ROS_INFO("resetting raw positions"); - } + P2OSPacket pkt; + unsigned char p2oscommand[4]; + + if(this->sippacket) { + this->sippacket->rawxpos = 0; + this->sippacket->rawypos = 0; + this->sippacket->xpos = 0; + this->sippacket->ypos = 0; + p2oscommand[0] = SETO; + p2oscommand[1] = ARGINT; + pkt.Build(p2oscommand, 2); + this->SendReceive(&pkt,false); + ROS_INFO("resetting raw positions"); + } } /* toggle sonars on/off, according to val */ void P2OSNode::ToggleSonarPower(unsigned char val) { - unsigned char command[4]; - P2OSPacket packet; - - command[0] = SONAR; - command[1] = ARGINT; - command[2] = val; - command[3] = 0; - packet.Build(command, 4); - SendReceive(&packet,false); + unsigned char command[4]; + P2OSPacket packet; + + command[0] = SONAR; + command[1] = ARGINT; + command[2] = val; + command[3] = 0; + packet.Build(command, 4); + SendReceive(&packet,false); } /*! \brief Toggle motors on/off. @@ -837,16 +835,16 @@ void P2OSNode::ToggleSonarPower(unsigned char val) */ void P2OSNode::ToggleMotorPower(unsigned char val) { - unsigned char command[4]; - P2OSPacket packet; - ROS_INFO("motor state: %d\n", p2os_data.motors.state); - p2os_data.motors.state = (int) val; - command[0] = ENABLE; - command[1] = ARGINT; - command[2] = val; - command[3] = 0; - packet.Build(command, 4); - SendReceive(&packet,false); + unsigned char command[4]; + P2OSPacket packet; + ROS_INFO("motor state: %d\n", p2os_data.motors.state); + p2os_data.motors.state = (int) val; + command[0] = ENABLE; + command[1] = ARGINT; + command[2] = val; + command[3] = 0; + packet.Build(command, 4); + SendReceive(&packet,false); } ///////////////////////////////////////////////////// @@ -857,85 +855,85 @@ void P2OSNode::ToggleMotorPower(unsigned char val) //! Convert ticks to degrees. inline double P2OSNode::TicksToDegrees (int joint, unsigned char ticks) { - if ((joint < 0) || (joint >= sippacket->armNumJoints)) - return 0; + if ((joint < 0) || (joint >= sippacket->armNumJoints)) + return 0; - double result; - int pos = ticks - sippacket->armJoints[joint].centre; - result = 90.0 / static_cast (sippacket->armJoints[joint].ticksPer90); - result = result * pos; - if ((joint >= 0) && (joint <= 2)) - result = -result; + double result; + int pos = ticks - sippacket->armJoints[joint].centre; + result = 90.0 / static_cast (sippacket->armJoints[joint].ticksPer90); + result = result * pos; + if ((joint >= 0) && (joint <= 2)) + result = -result; - return result; + return result; } // Degrees to ticks from the ARIA software //! convert degrees to ticks inline unsigned char P2OSNode::DegreesToTicks (int joint, double degrees) { - double val; + double val; - if ((joint < 0) || (joint >= sippacket->armNumJoints)) - return 0; + if ((joint < 0) || (joint >= sippacket->armNumJoints)) + return 0; - val = static_cast (sippacket->armJoints[joint].ticksPer90) * degrees / 90.0; - val = round (val); - if ((joint >= 0) && (joint <= 2)) - val = -val; - val += sippacket->armJoints[joint].centre; + val = static_cast (sippacket->armJoints[joint].ticksPer90) * degrees / 90.0; + val = round (val); + if ((joint >= 0) && (joint <= 2)) + val = -val; + val += sippacket->armJoints[joint].centre; - if (val < sippacket->armJoints[joint].min) return sippacket->armJoints[joint].min; - else if (val > sippacket->armJoints[joint].max) return sippacket->armJoints[joint].max; - else return static_cast (round (val)); + if (val < sippacket->armJoints[joint].min) return sippacket->armJoints[joint].min; + else if (val > sippacket->armJoints[joint].max) return sippacket->armJoints[joint].max; + else return static_cast (round (val)); } //! Convert ticks to radians inline double P2OSNode::TicksToRadians (int joint, unsigned char ticks) { - double result = DTOR (TicksToDegrees (joint, ticks)); - return result; + double result = DTOR (TicksToDegrees (joint, ticks)); + return result; } //! Convert radians to ticks. inline unsigned char P2OSNode::RadiansToTicks (int joint, double rads) { - unsigned char result = static_cast (DegreesToTicks (joint, RTOD (rads))); - return result; + unsigned char result = static_cast (DegreesToTicks (joint, RTOD (rads))); + return result; } //! Convert radians per second to radians per encoder tick. inline double P2OSNode::RadsPerSectoSecsPerTick (int joint, double speed) { - double degs = RTOD (speed); - double ticksPerDeg = static_cast (sippacket->armJoints[joint].ticksPer90) / 90.0; - double ticksPerSec = degs * ticksPerDeg; - double secsPerTick = 1000.0f / ticksPerSec; - - if (secsPerTick > 127) - return 127; - else if (secsPerTick < 1) - return 1; - return secsPerTick; + double degs = RTOD (speed); + double ticksPerDeg = static_cast (sippacket->armJoints[joint].ticksPer90) / 90.0; + double ticksPerSec = degs * ticksPerDeg; + double secsPerTick = 1000.0f / ticksPerSec; + + if (secsPerTick > 127) + return 127; + else if (secsPerTick < 1) + return 1; + return secsPerTick; } //! Convert Seconds per encoder tick to radians per second. inline double P2OSNode::SecsPerTicktoRadsPerSec (int joint, double msecs) { - double ticksPerSec = 1.0 / (static_cast (msecs) / 1000.0); - double ticksPerDeg = static_cast (sippacket->armJoints[joint].ticksPer90) / 90.0; - double degs = ticksPerSec / ticksPerDeg; - double rads = DTOR (degs); + double ticksPerSec = 1.0 / (static_cast (msecs) / 1000.0); + double ticksPerDeg = static_cast (sippacket->armJoints[joint].ticksPer90) / 90.0; + double degs = ticksPerSec / ticksPerDeg; + double rads = DTOR (degs); - return rads; + return rads; } void P2OSNode::SendPulse (void) { - unsigned char command; - P2OSPacket packet; + unsigned char command; + P2OSPacket packet; - command = PULSE; - packet.Build(&command, 1); - SendReceive(&packet); + command = PULSE; + packet.Build(&command, 1); + SendReceive(&packet); } diff --git a/p2os_driver/src/p2osnode.cc b/p2os_driver/src/p2osnode.cc index 2b31a1c..15e0db5 100644 --- a/p2os_driver/src/p2osnode.cc +++ b/p2os_driver/src/p2osnode.cc @@ -23,12 +23,13 @@ #include -#include +/* #include #include -#include +#include */ +#include #include -#include -#include +/* #include + #include */ int main(int argc, char** argv) { diff --git a/p2os_driver/src/packet.cc b/p2os_driver/src/packet.cc index e967512..ca703ab 100644 --- a/p2os_driver/src/packet.cc +++ b/p2os_driver/src/packet.cc @@ -27,7 +27,6 @@ #include #include #include /* for exit() */ -#include void P2OSPacket::Print() {