Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
5158318fcb |
@ -4,7 +4,7 @@ FROM athackst/ros2:foxy-dev
|
||||
#
|
||||
ENV DEBIAN_FRONTEND=noninteractive
|
||||
RUN apt-get update \
|
||||
&& apt-get -y install --no-install-recommends rsync ros-foxy-xacro ros-foxy-joint-state-publisher-gui ros-foxy-urg-node\
|
||||
&& apt-get -y install --no-install-recommends rsync \
|
||||
#
|
||||
# Clean up
|
||||
&& apt-get autoremove -y \
|
||||
@ -26,4 +26,4 @@ RUN SNIPPET="export PROMPT_COMMAND='history -a' && export HISTFILE=/commandhisto
|
||||
&& mkdir /commandhistory \
|
||||
&& touch /commandhistory/.bash_history \
|
||||
&& chown -R $USERNAME /commandhistory \
|
||||
&& echo $SNIPPET >> "/home/$USERNAME/.bashrc"
|
||||
&& echo $SNIPPET >> "/home/$USERNAME/.bashrc"
|
||||
2
Makefile
2
Makefile
@ -16,7 +16,7 @@ upload:
|
||||
# upload: .uploaded
|
||||
|
||||
run: upload
|
||||
ssh lab1_5@pionier6 -t -- docker run --rm --net host -it --device /dev/ttyS0 -v /home/lab1_5:/ws -v /dev/shm:/dev/shm irth7/ros2aria-dev /bin/bash /ws/ros2aria/run.sh
|
||||
ssh lab1_5@pionier6 -t -- docker run --rm --network=host -it --device /dev/ttyS0 -v /home/lab1_5:/ws irth7/ros2aria-dev /bin/bash /ws/ros2aria/run.sh
|
||||
|
||||
legacy:
|
||||
ssh lab1_5@pionier6 -t -- ./run.sh
|
||||
|
||||
@ -1,40 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(pionier_description)
|
||||
|
||||
# Default to C99
|
||||
if(NOT CMAKE_C_STANDARD)
|
||||
set(CMAKE_C_STANDARD 99)
|
||||
endif()
|
||||
|
||||
# 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 -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# uncomment the line when a copyright and license is not present in all source files
|
||||
#set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# uncomment the line when this package is not in a git repo
|
||||
#set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
install(
|
||||
DIRECTORY launch urdf meshes
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@ -1,57 +0,0 @@
|
||||
import launch
|
||||
from launch.substitutions import Command, LaunchConfiguration
|
||||
import launch_ros
|
||||
import os
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_share = launch_ros.substitutions.FindPackageShare(package='pionier_description').find('pionier_description')
|
||||
default_model_path = os.path.join(pkg_share, 'urdf/description.urdf.xacro')
|
||||
# default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')
|
||||
|
||||
robot_state_publisher_node = launch_ros.actions.Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
|
||||
)
|
||||
joint_state_publisher_node = launch_ros.actions.Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher',
|
||||
condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
|
||||
)
|
||||
joint_state_publisher_gui_node = launch_ros.actions.Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher',
|
||||
# condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
|
||||
)
|
||||
# rviz_node = launch_ros.actions.Node(
|
||||
# package='rviz2',
|
||||
# executable='rviz2',
|
||||
# name='rviz2',
|
||||
# output='screen',
|
||||
# arguments=['-d', LaunchConfiguration('rvizconfig')],
|
||||
# )
|
||||
|
||||
urg_node = Node(
|
||||
package='urg_node',
|
||||
executable='urg_node_driver',
|
||||
emulate_tty=True,
|
||||
parameters=[{'serial_port': '/dev/ttyACM0'}],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
return launch.LaunchDescription([
|
||||
urg_node,
|
||||
launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
|
||||
description='Flag to enable joint_state_publisher_gui'),
|
||||
launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
|
||||
description='Absolute path to robot urdf file'),
|
||||
# launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
|
||||
# description='Absolute path to rviz config file'),
|
||||
joint_state_publisher_node,
|
||||
joint_state_publisher_gui_node,
|
||||
robot_state_publisher_node,
|
||||
# rviz_node
|
||||
])
|
||||
File diff suppressed because one or more lines are too long
@ -1,18 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>pionier_description</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,164 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
|
||||
<!-- Define robot constants -->
|
||||
<xacro:property name="base_width" value="0.381"/>
|
||||
<xacro:property name="base_length" value="0.455"/>
|
||||
<xacro:property name="base_height" value="0.237"/>
|
||||
|
||||
<xacro:property name="wheel_radius" value="0.0975"/>
|
||||
<xacro:property name="wheel_width" value="0.05"/>
|
||||
<xacro:property name="wheel_ygap" value="0.025"/>
|
||||
<xacro:property name="wheel_zoff" value="0.05"/>
|
||||
<xacro:property name="wheel_xoff" value="0.0"/>
|
||||
|
||||
<xacro:property name="caster_xoff" value="0.15"/>
|
||||
|
||||
<!-- Define some commonly used intertial properties -->
|
||||
<xacro:macro name="box_inertia" params="m w h d">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
|
||||
<mass value="${m}"/>
|
||||
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="cylinder_inertia" params="m r h">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
|
||||
<mass value="${m}"/>
|
||||
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="sphere_inertia" params="m r">
|
||||
<inertial>
|
||||
<mass value="${m}"/>
|
||||
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- Robot Base -->
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${base_length} ${base_width} ${base_height}"/>
|
||||
</geometry>
|
||||
<material name="Cyan">
|
||||
<color rgba="0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="${base_length} ${base_width} ${base_height}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>
|
||||
</link>
|
||||
|
||||
<!-- Robot Footprint -->
|
||||
<link name="base_footprint">
|
||||
<xacro:box_inertia m="0" w="0" d="0" h="0"/>
|
||||
</link>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="base_footprint"/>
|
||||
<origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- Wheels -->
|
||||
<xacro:macro name="wheel" params="prefix x_reflect y_reflect">
|
||||
<link name="${prefix}_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
|
||||
</geometry>
|
||||
<material name="Gray">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="${prefix}_link"/>
|
||||
<origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
|
||||
<xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />
|
||||
|
||||
<!-- <link name="front_caster">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
|
||||
</geometry>
|
||||
<material name="Cyan">
|
||||
<color rgba="0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
|
||||
</link> -->
|
||||
|
||||
<!-- <joint name="caster_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="front_caster"/>
|
||||
<origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
|
||||
</joint> -->
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
<joint name="lidar_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="laser"/>
|
||||
<origin xyz="0.14 0 ${base_height + 0.065}" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="laser">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file://$(find pionier_description)/meshes/hokuyo.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="1e-5" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
@ -41,7 +41,7 @@ add_executable(ros2aria
|
||||
src/wheels.cpp
|
||||
src/clutch.cpp
|
||||
src/state.cpp)
|
||||
|
||||
|
||||
ament_target_dependencies(ros2aria rclcpp)
|
||||
ament_target_dependencies(ros2aria std_srvs)
|
||||
ament_target_dependencies(ros2aria geometry_msgs)
|
||||
@ -50,8 +50,7 @@ ament_target_dependencies(ros2aria nav_msgs)
|
||||
ament_target_dependencies(ros2aria tf2)
|
||||
ament_target_dependencies(ros2aria ros2aria_msgs)
|
||||
|
||||
target_include_directories(ros2aria PRIVATE
|
||||
include
|
||||
target_include_directories(ros2aria PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
/usr/local/Aria/include)
|
||||
|
||||
@ -1,41 +0,0 @@
|
||||
#pragma once
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
|
||||
#include "Aria/Aria.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class RAIIBot
|
||||
{
|
||||
public:
|
||||
typedef std::shared_ptr<RAIIBot> SharedPtr;
|
||||
|
||||
RAIIBot(rclcpp::Node *node, std::string port);
|
||||
|
||||
~RAIIBot();
|
||||
ArRobot *getRobot();
|
||||
|
||||
ArGripper *getGripper();
|
||||
rclcpp::Clock::SharedPtr getClock();
|
||||
|
||||
void lock();
|
||||
void unlock();
|
||||
void pokeWatchdog();
|
||||
private:
|
||||
ArRobot *robot = nullptr;
|
||||
ArRobotConnector *robotConn = nullptr;
|
||||
ArArgumentBuilder *args = nullptr;
|
||||
ArArgumentParser *argparser = nullptr;
|
||||
ArGripper *gripper = nullptr;
|
||||
rclcpp::Node *node = nullptr;
|
||||
rclcpp::TimerBase::SharedPtr watchdogTimer;
|
||||
rclcpp::Time lastWatchdogPoke;
|
||||
rclcpp::Clock::SharedPtr clock;
|
||||
|
||||
rclcpp::Logger get_logger();
|
||||
void startWatchdog();
|
||||
void watchdog_cb();
|
||||
};
|
||||
@ -17,8 +17,6 @@
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>ros2aria_msgs</depend>
|
||||
|
||||
<export>
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg)
|
||||
{
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
@ -14,8 +14,7 @@ void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
|
||||
auto r = robot->getRobot();
|
||||
r->setVel(x * 1e3);
|
||||
if (r->hasLatVel()){
|
||||
if (r->hasLatVel())
|
||||
r->setLatVel(y * 1e3);
|
||||
}
|
||||
r->setRotVel(z * 180 / M_PI);
|
||||
}
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "./ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
|
||||
{
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#include <cstdio>
|
||||
#include <memory>
|
||||
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
|
||||
#include "ros2aria.hpp"
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2/transform_datatypes.h"
|
||||
|
||||
inline void convert_transform_to_pose(const tf2::Transform &trans, geometry_msgs::msg::Pose &pose)
|
||||
{
|
||||
@ -29,23 +30,13 @@ nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp)
|
||||
pose.twist.twist.linear.y = r->getLatVel() / 1000;
|
||||
pose.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
|
||||
|
||||
pose.header.frame_id = "odom"; // TODO: use correct frame_id
|
||||
pose.child_frame_id = "base_link"; // TODO: use correct child_frame_id
|
||||
pose.header.frame_id = "header.frame_id"; // TODO: use correct frame_id
|
||||
pose.child_frame_id = "child_frame_id"; // TODO: use correct child_frame_id
|
||||
pose.header.stamp = stamp;
|
||||
|
||||
sendOdomTransform(stamp, geometry_msgs::msg::Transform(rotation, position));
|
||||
return pose;
|
||||
}
|
||||
|
||||
void Ros2Aria::sendOdomTransform(rclcpp::Time stamp, tf2::Transform transform){
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
t.header.stamp = stamp;
|
||||
t.header.frame_id = "odom";
|
||||
t.child_frame_id = "base_link";
|
||||
t.transform = transform;
|
||||
odom_tf_broadcaster_->sendTransform(t);
|
||||
}
|
||||
|
||||
void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose)
|
||||
{
|
||||
if (this->pose_pub_->get_subscription_count() == 0)
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::publish()
|
||||
{
|
||||
|
||||
@ -1,105 +1,143 @@
|
||||
#include "ros2aria/raiibot.hpp"
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
|
||||
#include "Aria/Aria.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
RAIIBot::RAIIBot(rclcpp::Node *node, std::string port) {
|
||||
this->robot = new ArRobot();
|
||||
this->gripper = new ArGripper(this->robot);
|
||||
this->node = node;
|
||||
this->clock = rclcpp::Clock::make_shared();
|
||||
class RAIIBot
|
||||
{
|
||||
public:
|
||||
typedef std::shared_ptr<RAIIBot> SharedPtr;
|
||||
|
||||
args = new ArArgumentBuilder();
|
||||
this->argparser = new ArArgumentParser(args);
|
||||
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
|
||||
args->add("-robotPort /dev/ttyS0"); // pass robot's serial port to Aria // TODO: use `port` variable.
|
||||
// args->add("-robotLogPacketsReceived"); // log received packets
|
||||
// args->add("-robotLogPacketsSent"); // log sent packets
|
||||
// args->add("-robotLogVelocitiesReceived"); // log received velocities
|
||||
// args->add("-robotLogMovementSent");
|
||||
// args->add("-robotLogMovementReceived");
|
||||
this->robotConn = new ArRobotConnector(argparser, robot);
|
||||
if (!this->robotConn->connectRobot()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
|
||||
// rclcpp::shutdown();
|
||||
RAIIBot(rclcpp::Node *node, std::string port)
|
||||
{
|
||||
this->robot = new ArRobot();
|
||||
this->gripper = new ArGripper(this->robot);
|
||||
this->node = node;
|
||||
this->clock = rclcpp::Clock::make_shared();
|
||||
|
||||
args = new ArArgumentBuilder();
|
||||
this->argparser = new ArArgumentParser(args);
|
||||
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
|
||||
args->add("-robotPort /dev/ttyS0"); // pass robot's serial port to Aria // TODO: use `port` variable.
|
||||
// args->add("-robotLogPacketsReceived"); // log received packets
|
||||
// args->add("-robotLogPacketsSent"); // log sent packets
|
||||
// args->add("-robotLogVelocitiesReceived"); // log received velocities
|
||||
// args->add("-robotLogMovementSent");
|
||||
// args->add("-robotLogMovementReceived");
|
||||
this->robotConn = new ArRobotConnector(argparser, robot);
|
||||
if (!this->robotConn->connectRobot())
|
||||
{
|
||||
|
||||
// RCLCPP_INFO(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
|
||||
// rclcpp::shutdown();
|
||||
}
|
||||
|
||||
this->robot->runAsync(true);
|
||||
this->robot->enableMotors();
|
||||
|
||||
this->startWatchdog();
|
||||
}
|
||||
|
||||
this->robot->runAsync(true);
|
||||
this->robot->enableMotors();
|
||||
~RAIIBot()
|
||||
{
|
||||
std::cout << std::endl
|
||||
<< "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
|
||||
|
||||
this->startWatchdog();
|
||||
}
|
||||
if (this->robot != nullptr)
|
||||
{
|
||||
this->robot->lock();
|
||||
std::cout << "disabling motors" << std::endl;
|
||||
this->robot->disableMotors();
|
||||
std::cout << "disabled motors" << std::endl;
|
||||
Aria::shutdown();
|
||||
}
|
||||
if (this->robotConn != nullptr)
|
||||
{
|
||||
std::cout << "disconnecting" << std::endl;
|
||||
this->robotConn->disconnectAll();
|
||||
std::cout << "disconnected" << std::endl;
|
||||
}
|
||||
|
||||
RAIIBot::~RAIIBot() {
|
||||
std::cout << std::endl
|
||||
<< "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
|
||||
|
||||
if (this->robot != nullptr) {
|
||||
this->robot->lock();
|
||||
std::cout << "disabling motors" << std::endl;
|
||||
this->robot->disableMotors();
|
||||
std::cout << "disabled motors" << std::endl;
|
||||
Aria::shutdown();
|
||||
}
|
||||
if (this->robotConn != nullptr) {
|
||||
std::cout << "disconnecting" << std::endl;
|
||||
this->robotConn->disconnectAll();
|
||||
std::cout << "disconnected" << std::endl;
|
||||
if (this->args != nullptr)
|
||||
delete this->args;
|
||||
if (this->argparser != nullptr)
|
||||
delete this->argparser;
|
||||
if (this->robotConn != nullptr)
|
||||
delete this->robotConn;
|
||||
if (this->robot != nullptr)
|
||||
delete this->robot;
|
||||
}
|
||||
|
||||
if (this->args != nullptr)
|
||||
delete this->args;
|
||||
if (this->argparser != nullptr)
|
||||
delete this->argparser;
|
||||
if (this->robotConn != nullptr)
|
||||
delete this->robotConn;
|
||||
if (this->robot != nullptr)
|
||||
delete this->robot;
|
||||
}
|
||||
|
||||
ArRobot *RAIIBot::getRobot() {
|
||||
return this->robot;
|
||||
}
|
||||
|
||||
ArGripper *RAIIBot::getGripper() {
|
||||
return this->gripper;
|
||||
}
|
||||
|
||||
rclcpp::Clock::SharedPtr RAIIBot::getClock() {
|
||||
return this->clock;
|
||||
}
|
||||
|
||||
void RAIIBot::lock() {
|
||||
if (this->robot != nullptr)
|
||||
this->robot->lock();
|
||||
}
|
||||
|
||||
void RAIIBot::unlock() {
|
||||
if (this->robot != nullptr)
|
||||
this->robot->unlock();
|
||||
}
|
||||
|
||||
void RAIIBot::pokeWatchdog() {
|
||||
// this should probably be made thread safe, or something?
|
||||
this->lastWatchdogPoke = this->clock->now();
|
||||
}
|
||||
|
||||
rclcpp::Logger RAIIBot::get_logger() {
|
||||
return this->node->get_logger();
|
||||
}
|
||||
|
||||
void RAIIBot::startWatchdog() {
|
||||
this->lastWatchdogPoke = clock->now();
|
||||
watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
|
||||
}
|
||||
|
||||
void RAIIBot::watchdog_cb() {
|
||||
auto now = this->clock->now();
|
||||
if (now - this->lastWatchdogPoke > 1s) {
|
||||
std::lock_guard<RAIIBot> lock(*this);
|
||||
auto r = this->getRobot();
|
||||
r->setVel(0);
|
||||
r->setRotVel(0);
|
||||
if (r->hasLatVel())
|
||||
r->setLatVel(0);
|
||||
ArRobot *getRobot()
|
||||
{
|
||||
return this->robot;
|
||||
}
|
||||
}
|
||||
|
||||
ArGripper *getGripper()
|
||||
{
|
||||
return this->gripper;
|
||||
}
|
||||
|
||||
rclcpp::Clock::SharedPtr getClock()
|
||||
{
|
||||
return this->clock;
|
||||
}
|
||||
|
||||
void lock()
|
||||
{
|
||||
if (this->robot != nullptr)
|
||||
this->robot->lock();
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
if (this->robot != nullptr)
|
||||
this->robot->unlock();
|
||||
}
|
||||
|
||||
void pokeWatchdog()
|
||||
{
|
||||
// this should probably be made thread safe, or something?
|
||||
this->lastWatchdogPoke = this->clock->now();
|
||||
}
|
||||
|
||||
private:
|
||||
ArRobot *robot = nullptr;
|
||||
ArRobotConnector *robotConn = nullptr;
|
||||
ArArgumentBuilder *args = nullptr;
|
||||
ArArgumentParser *argparser = nullptr;
|
||||
ArGripper *gripper = nullptr;
|
||||
rclcpp::Node *node = nullptr;
|
||||
rclcpp::TimerBase::SharedPtr watchdogTimer;
|
||||
rclcpp::Time lastWatchdogPoke;
|
||||
rclcpp::Clock::SharedPtr clock;
|
||||
|
||||
rclcpp::Logger get_logger()
|
||||
{
|
||||
return this->node->get_logger();
|
||||
}
|
||||
|
||||
void startWatchdog()
|
||||
{
|
||||
this->lastWatchdogPoke = clock->now();
|
||||
watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
|
||||
}
|
||||
|
||||
void watchdog_cb()
|
||||
{
|
||||
auto now = this->clock->now();
|
||||
if (now - this->lastWatchdogPoke > 1s)
|
||||
{
|
||||
std::lock_guard<RAIIBot> lock(*this);
|
||||
auto r = this->getRobot();
|
||||
r->setVel(0);
|
||||
r->setRotVel(0);
|
||||
if (r->hasLatVel())
|
||||
r->setLatVel(0);
|
||||
}
|
||||
}
|
||||
};
|
||||
@ -1,4 +1,4 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
using std::placeholders::_1;
|
||||
using std::placeholders::_2;
|
||||
@ -19,7 +19,6 @@ Ros2Aria::Ros2Aria()
|
||||
sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
|
||||
|
||||
pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
|
||||
odom_tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
|
||||
|
||||
wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000);
|
||||
|
||||
@ -47,7 +46,8 @@ Ros2Aria::~Ros2Aria()
|
||||
r->remSensorInterpTask("ROSPublishingTask");
|
||||
}
|
||||
|
||||
void Ros2Aria::stop(const std_srvs::srv::Empty::Response::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
|
||||
void Ros2Aria::stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const
|
||||
{
|
||||
UNUSED(request);
|
||||
UNUSED(response);
|
||||
RCLCPP_INFO(this->get_logger(), "stop");
|
||||
|
||||
@ -1,20 +1,18 @@
|
||||
#include "Aria/Aria.h"
|
||||
#include "geometry_msgs/msg/transform.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "raiibot.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "ros2aria_msgs/msg/robot_info_msg.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "Aria/Aria.h"
|
||||
|
||||
#include "sensor_msgs/msg/point_cloud.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "std_msgs/msg/float32.hpp"
|
||||
#include "std_msgs/msg/int8.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "std_srvs/srv/empty.hpp"
|
||||
#include "tf2/transform_datatypes.h"
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "std_msgs/msg/int8.hpp"
|
||||
#include "std_msgs/msg/float32.hpp"
|
||||
|
||||
#include "ros2aria_msgs/msg/robot_info_msg.hpp"
|
||||
#include "./raiibot.cpp"
|
||||
|
||||
#define UNUSED(x) (void)(x)
|
||||
|
||||
@ -45,9 +43,6 @@ private:
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_;
|
||||
void publishPose(nav_msgs::msg::Odometry pose);
|
||||
|
||||
void sendOdomTransform(rclcpp::Time stamp, tf2::Transform transform);
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> odom_tf_broadcaster_;
|
||||
|
||||
sensor_msgs::msg::JointState handleWheels(rclcpp::Time stamp);
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_;
|
||||
void publishWheels(sensor_msgs::msg::JointState wheels);
|
||||
@ -66,7 +61,7 @@ private:
|
||||
|
||||
// services
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
|
||||
void stop(const std_srvs::srv::Empty::Response::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
|
||||
void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const;
|
||||
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_open_service_;
|
||||
void gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
|
||||
@ -1,4 +1,4 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
#include "sensor_msgs/point_cloud_conversion.hpp"
|
||||
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
void Ros2Aria::publishState(rclcpp::Time stamp)
|
||||
{
|
||||
@ -31,7 +31,9 @@ void Ros2Aria::publishState(rclcpp::Time stamp)
|
||||
robot_info_msg.twist.linear.x = r->getVel() / 1000;
|
||||
robot_info_msg.twist.linear.y = r->getLatVel() / 1000.0;
|
||||
robot_info_msg.twist.angular.z = r->getRotVel() * M_PI / 180;
|
||||
// TODO: actually keep track of robot state
|
||||
|
||||
// TODO: actually keep track of robot state (true -> unlocked, false ->
|
||||
// locked). This requires safety plugin to be implemented
|
||||
robot_info_msg.state.data = true;
|
||||
robot_info_msg.clutch.data = r->areMotorsEnabled();
|
||||
// TODO: actually look for obstacles
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include "ros2aria/ros2aria.hpp"
|
||||
#include "ros2aria.hpp"
|
||||
|
||||
sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user