sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-25 17:22:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added a joint position publisher. It currently publishes all joint angles for the robotic arm, but could be adapted to a different model

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-03 20:57:47 -04:00
parent 4e30756b75
commit a881191bc6
5 changed files with 52 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
rrrobot_src/src/simulation_drivers/model_joints.world Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<sdf version="1.6">
<world name="default">
<!-- Ground Plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://fanuc_robotic_arm</uri>
</include>
</world>
</sdf>

18
rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt
View File

@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
gazebo_ros
message_generation
)
find_package(gazebo REQUIRED)
@@ -48,11 +49,10 @@ find_package(gazebo REQUIRED)
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
add_message_files(
FILES
arm_angles.msg
)
## Generate services in the 'srv' folder
# add_service_files(
@@ -69,10 +69,10 @@ find_package(gazebo REQUIRED)
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
generate_messages(
DEPENDENCIES
std_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##

6
rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_angles.msg Normal file
View File

@@ -0,0 +1,6 @@
float32 shoulder_pivot_angle
float32 shoulder_joint_angle
float32 elbow_joint_angle
float32 wrist_pivot_angle
float32 wrist_joint_angle
float32 end_effector_pivot_angle

2
rrrobot_src/src/simulation_drivers/src/simulation_env/package.xml
View File

@@ -52,12 +52,14 @@
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>gazebo_ros</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->

26
rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp
View File

@@ -5,6 +5,7 @@
#include <iostream>
#include "ros/ros.h"
#include "simulation_env/arm_angles.h"
using std::cout;
using std::endl;
@@ -29,7 +30,7 @@ namespace gazebo
}
nh.reset(new ros::NodeHandle("arm_node"));
publisher = nh->advertise<std_msgs::Float32>("arm_positions", 1);
publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1);
}
private:
@@ -37,18 +38,25 @@ namespace gazebo
void onUpdate()
{
//cout << __func__ << endl;
std_msgs::Float32 msg;
msg.data = 0.0f;
gazebo::physics::BasePtr joint = model->GetChild("shoulder_joint");
simulation_env::arm_angles msg;
//msg.data = 0.0f;
msg.shoulder_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_pivot").get())->Position();
msg.shoulder_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_joint").get())->Position();
msg.elbow_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("elbow_joint").get())->Position();
msg.wrist_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("wrist_pivot").get())->Position();
msg.wrist_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("wrist_joint").get())->Position();
msg.end_effector_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("end_effector_pivot").get())->Position();
//gazebo::physics::BasePtr joint = model->GetChild("shoulder_joint");
//cout << "Joint type: " << joint->GetType() << "joint type & EntityType::JOINT = " << (joint->GetType() & gazebo::physics::Base::EntityType::JOINT) << endl;
if((joint->GetType() & gazebo::physics::Base::EntityType::JOINT) != 0)
{
//if((joint->GetType() & gazebo::physics::Base::EntityType::JOINT) != 0)
//{
//cout << "Got an element of type JOINT" << endl;
//cout << "Joint Position: " << dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position() << endl;
msg.data = dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position();
//msg.data = dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position();
//cout << "Position: " << msg.data << endl;
}
if(msg.data != 0.0f) publisher.publish(msg);
//}
/*if(msg.data != 0.0f) */publisher.publish(msg);
ros::spinOnce();
}
physics::ModelPtr model;